u32_t criticalLoop(u32_t count)
{
s64_t mseconds;
mseconds = k_uptime_get();
while (k_uptime_get() < mseconds + NUM_MILLISECONDS) {
struct k_work work_item;
k_work_init(&work_item, criticalRtn);
k_work_submit_to_queue(&offload_work_q, &work_item);
count++;
}
return count;
}
static inline void encode_hdr(struct bt_monitor_hdr *hdr, u16_t opcode,
u16_t len)
{
struct bt_monitor_ts32 *ts;
hdr->opcode = sys_cpu_to_le16(opcode);
hdr->flags = 0;
ts = (void *)hdr->ext;
ts->type = BT_MONITOR_TS32;
ts->ts32 = sys_cpu_to_le32(k_uptime_get() * 10);
hdr->hdr_len = sizeof(*ts);
encode_drops(hdr, BT_MONITOR_COMMAND_DROPS, &drops.cmd);
encode_drops(hdr, BT_MONITOR_EVENT_DROPS, &drops.evt);
encode_drops(hdr, BT_MONITOR_ACL_TX_DROPS, &drops.acl_tx);
encode_drops(hdr, BT_MONITOR_ACL_RX_DROPS, &drops.acl_rx);
#if defined(CONFIG_BT_BREDR)
encode_drops(hdr, BT_MONITOR_SCO_TX_DROPS, &drops.sco_tx);
encode_drops(hdr, BT_MONITOR_SCO_RX_DROPS, &drops.sco_rx);
#endif
encode_drops(hdr, BT_MONITOR_OTHER_DROPS, &drops.other);
hdr->data_len = sys_cpu_to_le16(4 + hdr->hdr_len + len);
}
/* Precalculate all three rows of an image and start the rendering. */
static void start_image(struct mb_display *disp, const struct mb_image *img)
{
int row, col;
for (row = 0; row < DISPLAY_ROWS; row++) {
disp->row[row] = 0;
for (col = 0; col < DISPLAY_COLS; col++) {
if (GET_PIXEL(img, map[row][col].x, map[row][col].y)) {
disp->row[row] |= BIT(LED_COL1_GPIO_PIN + col);
}
}
disp->row[row] = ~disp->row[row] & col_mask;
disp->row[row] |= BIT(LED_ROW1_GPIO_PIN + row);
}
disp->cur = 0;
if (disp->duration == K_FOREVER) {
disp->expiry = K_FOREVER;
} else {
disp->expiry = k_uptime_get() + disp->duration;
}
k_timer_start(&disp->timer, K_NO_WAIT, K_MSEC(4));
}
/* a thread sleeps and times out, then reports through a fifo */
static void test_thread_sleep(void *delta, void *arg2, void *arg3)
{
s64_t timestamp;
int timeout = (int)delta;
ARG_UNUSED(arg2);
ARG_UNUSED(arg3);
TC_PRINT(" thread sleeping for %d milliseconds\n", timeout);
timestamp = k_uptime_get();
k_sleep(timeout);
timestamp = k_uptime_get() - timestamp;
TC_PRINT(" thread back from sleep\n");
if (timestamp < timeout || timestamp > timeout + 10) {
return;
}
k_sem_give(&reply_timeout);
}
/* a thread sleeps and times out, then reports through a fifo */
static void test_thread_sleep(void *delta, void *arg2, void *arg3)
{
s64_t timestamp;
int timeout = (int)delta;
ARG_UNUSED(arg2);
ARG_UNUSED(arg3);
TC_PRINT(" thread sleeping for %d milliseconds\n", timeout);
timestamp = k_uptime_get();
k_sleep(timeout);
timestamp = k_uptime_get() - timestamp;
TC_PRINT(" thread back from sleep\n");
if (timestamp < timeout || timestamp > timeout + __ticks_to_ms(2)) {
TC_ERROR("timestamp out of range, got %d\n", (int)timestamp);
return;
}
k_sem_give(&reply_timeout);
}
static inline void encode_hdr(struct bt_monitor_hdr *hdr, uint16_t opcode,
uint16_t len)
{
uint32_t ts32;
hdr->hdr_len = sizeof(hdr->type) + sizeof(hdr->ts32);
hdr->data_len = sys_cpu_to_le16(4 + hdr->hdr_len + len);
hdr->opcode = sys_cpu_to_le16(opcode);
hdr->flags = 0;
/* Extended header */
hdr->type = BT_MONITOR_TS32;
ts32 = k_uptime_get() * 10;
hdr->ts32 = sys_cpu_to_le32(ts32);
}
static int sm_registration_done(int index)
{
int ret = 0;
bool forced_update;
/* check for lifetime seconds - 1 so that we can update early */
if (clients[index].registered &&
(clients[index].trigger_update ||
((clients[index].lifetime - SECONDS_TO_UPDATE_EARLY) <=
(k_uptime_get() - clients[index].last_update) / 1000))) {
forced_update = clients[index].trigger_update;
clients[index].trigger_update = 0;
ret = sm_send_registration(index, forced_update,
do_update_reply_cb);
if (!ret) {
set_sm_state(index, ENGINE_UPDATE_SENT);
} else {
SYS_LOG_ERR("Registration update err: %d", ret);
}
}
return ret;
}
static int sm_bootstrap_done(int index)
{
/* TODO: Fix this */
/* check that we should still use bootstrap */
if (clients[index].use_bootstrap) {
#ifdef CONFIG_LWM2M_SECURITY_OBJ_SUPPORT
int i;
SYS_LOG_DBG("*** Bootstrap - checking for server info ...");
/* get the server URI */
if (sec_data->server_uri_len > 0) {
/* TODO: Write endpoint parsing function */
#if 0
if (!parse_endpoint(sec_data->server_uri,
sec_data->server_uri_len,
&clients[index].reg_server)) {
#else
if (true) {
#endif
SYS_LOG_ERR("Failed to parse URI!");
} else {
clients[index].has_registration_info = 1;
clients[index].registered = 0;
clients[index].bootstrapped++;
}
} else {
SYS_LOG_ERR("** failed to parse URI");
}
/* if we did not register above - then fail this and restart */
if (clients[index].bootstrapped == 0) {
/* Not ready - Retry with the bootstrap server again */
set_sm_state(index, ENGINE_DO_BOOTSTRAP);
} else {
set_sm_state(index, ENGINE_DO_REGISTRATION);
}
} else {
#endif
set_sm_state(index, ENGINE_DO_REGISTRATION);
}
return 0;
}
static int sm_send_registration(int index, bool send_obj_support_data,
zoap_reply_t reply_cb)
{
struct zoap_packet request;
struct net_pkt *pkt = NULL;
struct zoap_pending *pending = NULL;
struct zoap_reply *reply = NULL;
u8_t *payload;
u16_t client_data_len, len;
int ret = 0;
/* remember the last reg time */
clients[index].last_update = k_uptime_get();
ret = lwm2m_init_message(clients[index].net_ctx,
&request, &pkt, ZOAP_TYPE_CON,
ZOAP_METHOD_POST, 0, NULL, 0);
if (ret) {
goto cleanup;
}
zoap_add_option(&request, ZOAP_OPTION_URI_PATH,
LWM2M_RD_CLIENT_URI,
strlen(LWM2M_RD_CLIENT_URI));
if (!clients[index].registered) {
/* include client endpoint in URI QUERY on 1st registration */
zoap_add_option_int(&request, ZOAP_OPTION_CONTENT_FORMAT,
LWM2M_FORMAT_APP_LINK_FORMAT);
snprintf(query_buffer, sizeof(query_buffer) - 1,
"lwm2m=%s", LWM2M_PROTOCOL_VERSION);
zoap_add_option(&request, ZOAP_OPTION_URI_QUERY,
query_buffer, strlen(query_buffer));
snprintf(query_buffer, sizeof(query_buffer) - 1,
"ep=%s", clients[index].ep_name);
zoap_add_option(&request, ZOAP_OPTION_URI_QUERY,
query_buffer, strlen(query_buffer));
} else {
/* include server endpoint in URI PATH otherwise */
zoap_add_option(&request, ZOAP_OPTION_URI_PATH,
clients[index].server_ep,
strlen(clients[index].server_ep));
}
snprintf(query_buffer, sizeof(query_buffer) - 1,
"lt=%d", clients[index].lifetime);
zoap_add_option(&request, ZOAP_OPTION_URI_QUERY,
query_buffer, strlen(query_buffer));
/* TODO: add supported binding query string */
if (send_obj_support_data) {
/* generate the rd data */
client_data_len = lwm2m_get_rd_data(client_data,
sizeof(client_data));
payload = zoap_packet_get_payload(&request, &len);
if (!payload) {
ret = -EINVAL;
goto cleanup;
}
memcpy(payload, client_data, client_data_len);
ret = zoap_packet_set_used(&request, client_data_len);
if (ret) {
goto cleanup;
}
}
pending = lwm2m_init_message_pending(&request,
&clients[index].reg_server,
pendings, NUM_PENDINGS);
if (!pending) {
ret = -ENOMEM;
goto cleanup;
}
reply = zoap_reply_next_unused(replies, NUM_REPLIES);
if (!reply) {
SYS_LOG_ERR("No resources for waiting for replies.");
ret = -ENOMEM;
goto cleanup;
}
zoap_reply_init(reply, &request);
reply->reply = reply_cb;
/* log the registration attempt */
SYS_LOG_DBG("registration sent [%s]",
lwm2m_sprint_ip_addr(&clients[index].reg_server));
ret = lwm2m_udp_sendto(pkt, &clients[index].reg_server);
if (ret < 0) {
SYS_LOG_ERR("Error sending LWM2M packet (err:%d).",
ret);
goto cleanup;
}
zoap_pending_cycle(pending);
k_delayed_work_submit(&retransmit_work, pending->timeout);
return ret;
cleanup:
lwm2m_init_message_cleanup(pkt, pending, reply);
return ret;
}
static int sm_do_registration(int index)
{
int ret = 0;
if (clients[index].use_registration &&
!clients[index].registered &&
clients[index].has_registration_info) {
ret = sm_send_registration(index, true,
do_registration_reply_cb);
if (!ret) {
set_sm_state(index, ENGINE_REGISTRATION_SENT);
} else {
SYS_LOG_ERR("Registration err: %d", ret);
}
}
return ret;
}
