void TrackedDeviceList::update(const uint8_t * addrs_ptr, int8_t rssi) {
//bool found = false;
//#ifdef PRINT_TRACKEDDEVICES_VERBOSE
// LOGd("scanned: [%02X %02X %02X %02X %02X %02X], rssi: %d",
// addrs_ptr[5], addrs_ptr[4], addrs_ptr[3], addrs_ptr[2],
// addrs_ptr[1], addrs_ptr[0], rssi);
//#endif
for (int i = 0; i < getSize(); ++i) {
if (memcmp(addrs_ptr, _buffer->list[i].addr, BLE_GAP_ADDR_LEN) == 0) {
if (rssi >= _buffer->list[i].rssiThreshold) {
_buffer->counters[i] = 0;
#ifdef PRINT_TRACKEDDEVICES_VERBOSE
LOGd("Tracked device present nearby (%i >= %i)", rssi, _buffer->list[i].rssiThreshold);
#endif
} else {
#ifdef PRINT_TRACKEDDEVICES_VERBOSE
LOGd("Tracked device found, but not nearby (%i < %i)", rssi, _buffer->list[i].rssiThreshold);
#endif
}
//found = true;
break;
}
}
/*
if (!found) {
LOGd("Device found, but not to-be tracked:");
print(addrs_ptr);
LOGd("To-be tracked devices:");
for (int i = 0; i < getSize(); ++i) {
print(_buffer->list[i].addr);
}
}
*/
}
static void run_kthread_test(unsigned int n_threads)
{
unsigned int i;
LOGd("run_test begin.\n");
ASSERT(n_threads > 0);
for (i = 0; i < n_threads; i++) {
tdata_[i].id = i;
clear_bit(THREAD_WAKEUP, &tdata_[i].flags);
init_waitqueue_head(&tdata_[i].wait_q);
tdata_[i].timeout_ms = MAX_SCHEDULE_TIMEOUT;
LOGd("tdata id %u flags %lu wait_q %p timeout %lu.\n",
tdata_[i].id, tdata_[i].flags, &tdata_[i].wait_q, tdata_[i].timeout_ms);
tdata_[i].tsk = kthread_run(worker, &tdata_[i], "test_worker%u", i);
ASSERT(tdata_[i].tsk);
}
msleep_interruptible(1000);
for (i = 0; i < n_threads; i++) {
LOGd("wake up tdata_[%u].\n", i);
set_bit(THREAD_WAKEUP, &tdata_[i].flags);
wake_up_interruptible(&tdata_[i].wait_q);
}
msleep_interruptible(1000);
for (i = 0; i < n_threads; i++) {
kthread_stop(tdata_[i].tsk);
}
LOGd("run_test end.\n");
}
void resume_notifications() {
waiting_notification_t* notification = nb_peek();
if (send_notification(notification) == NRF_SUCCESS) {
#ifdef PRINT_MESH_VERBOSE
LOGd("notification done");
#endif
nb_pop();
if (nb_empty()) {
notifactionsPending = false;
#ifdef PRINT_MESH_VERBOSE
LOGd("no more notifications pending");
#endif
} else {
#ifdef PRINT_MESH_VERBOSE
LOGd("continue with next pending notification");
#endif
resume_notifications();
}
}
}
void Tracker::tick() {
if (_tracking) {
//! This function checks the counter for each device
//! If no device is nearby, turn off the light
bool deviceNearby = false;
if (_trackedDeviceList != NULL) {
deviceNearby = (_trackedDeviceList->isNearby() == TDL_IS_NEARBY);
}
//! Only send event on change of nearby state
if (deviceNearby && !_trackIsNearby) {
LOGd("send event: is nearby");
EventDispatcher::getInstance().dispatch(EVT_TRACKED_DEVICE_IS_NEARBY);
} else if (!deviceNearby && _trackIsNearby) {
LOGd("send event: is not nearby");
EventDispatcher::getInstance().dispatch(EVT_TRACKED_DEVICE_NOT_NEARBY);
}
_trackIsNearby = deviceNearby;
Timer::getInstance().start(_appTimerId, HZ_TO_TICKS(TRACKER_UPATE_FREQUENCY), this);
}
}
void State::print() {
#ifdef PRINT_DEBUG
LOGd("switch state:");
#ifdef SWITCH_STATE_PERSISTENT
_switchState->print();
LOGd("switch cache: %u", _switchStateCache);
#endif
LOGd("reset counter:");
_resetCounter->print();
LOGd("accumulated power:");
_accumulatedEnergy->print();
#endif
}
static void pstorage_callback_handler(pstorage_handle_t * handle, uint8_t op_code, uint32_t result, uint8_t * p_data,
uint32_t data_len) {
// we might want to check if things are actually stored, by using this callback
if (result != NRF_SUCCESS) {
LOGd("ERR_CODE: %d (0x%X)", result, result);
APP_ERROR_CHECK(result);
if (op_code == PSTORAGE_LOAD_OP_CODE) {
LOGd("Error with loading data");
}
} else {
LOGi("Opcode %i executed (no error)", op_code);
}
}
void IBeacon::setTxPower(int8_t txPower) {
#ifdef PRINT_IBEACON_VERBOSE
LOGd(FMT_SET_INT_VAL, "tx power", txPower);
#endif
_params.txPower = txPower;
}
void IBeacon::setMinor(uint16_t minor) {
#ifdef PRINT_IBEACON_VERBOSE
LOGd(FMT_SET_INT_VAL, "minor", minor);
#endif
_params.minor = BLEutil::convertEndian16(minor);
}
void set_tab( termstate_t *term, int index )
{
int i=0;
int j=0;
if (term->tab_last == MAX_TABS-1)
{
printf("cannot set any more tabulators. \n");
return;
}
LOGd(_DEBUG, "setting tab at %d", index );
// to keep the array sorted, skip all entries for
// tabs before current pos
while ( (term->tabs[i]<index) && (i<=term->tab_last) )
i++;
// now shift all remaining entries one step to the right
for (j=term->tab_last+1 ; j > i; j-- )
{
term->tabs[j] = term->tabs[j-1];
}
// now we can store index in tabs[i]
term->tabs[i] = index;
// increment tab_last
term->tab_last++;
return;
}
/**
* Set device name.
*
* @wdev walb device.
* @minor minor id. This will be used for default name.
* @name Name to set.
* If null or empty string is given and
* the preset name is empty,
* default name will be set using minor id.
*
* @return 0 in success, or -1.
*/
int walb_set_name(struct walb_dev *wdev,
unsigned int minor, const char *name)
{
int name_len;
char *dev_name;
ASSERT(wdev);
ASSERT(wdev->lsuper0);
dev_name = get_super_sector(wdev->lsuper0)->name;
if (name && *name) {
memset(dev_name, 0, DISK_NAME_LEN);
snprintf(dev_name, DISK_NAME_LEN, "%s", name);
} else if (*dev_name == 0) {
memset(dev_name, 0, DISK_NAME_LEN);
snprintf(dev_name, DISK_NAME_LEN, "%u", minor / 2);
}
LOGd("minor %u dev_name: %s\n", minor, dev_name);
name_len = strlen(dev_name);
ASSERT(name_len < DISK_NAME_LEN);
if (name_len > WALB_DEV_NAME_MAX_LEN) {
LOGe("Device name is too long: %s.\n", name);
return -1;
}
return 0;
}
int32_t
ferret_monitor_detach_component(CORBA_Object _dice_corba_obj,
uint16_t major,
uint16_t minor,
uint16_t instance,
CORBA_Server_Environment *_dice_corba_env)
{
local_object_id_t id;
l4vfs_th_node_t * node;
int ret;
char path[MAX_PATH];
l4dm_dataspace_t ds;
snprintf(path, MAX_PATH, "/%d/%d/%d", major, minor, instance);
id = l4vfs_th_resolve(0, path);
if (id == L4VFS_ILLEGAL_OBJECT_ID)
{
LOGd(verbose, "Request for unknown sensor");
return -1; // not found
}
node = l4vfs_th_node_for_id(id);
ds = ((sensor_entry_t *)(node->data))->ds;
ret = l4dm_revoke(&ds, *_dice_corba_obj, L4DM_ALL_RIGHTS);
if (ret)
{
LOG("Cannot revoke access rights for ds: ret = %d", ret);
return -1;
}
node->usage_count--;
sensors_check_and_free(node);
return 0;
}
bool Connection::send(OutputMessage_ptr msg) {
LOGt("Connection::send()");
boost::recursive_mutex::scoped_lock lock(m_connectionLock);
if(m_connectionState != CONNECTION_STATE_OPEN || m_writeError)
{
return false;
}
TRACK_MESSAGE(msg);
if(!m_pendingWrite)
{
if(msg->getProtocol())
msg->getProtocol()->onSendMessage(msg);
internalSend(msg);
}
else if(m_pendingWrite > 100 && server.configManager().getBool(ConfigManager::FORCE_CLOSE_SLOW_CONNECTION))
{
LOGd("Forcing slow connection to disconnect!");
close();
}
else
{
OutputMessagePool::getInstance()->autoSend(msg);
}
return true;
}
void TrackedDeviceList::print() const {
for (int i = 0; i < getSize(); ++i) {
LOGd("[%02X %02X %02X %02X %02X %02X]\trssi: %d\tcount: %d", _buffer->list[i].addr[5],
_buffer->list[i].addr[4], _buffer->list[i].addr[3], _buffer->list[i].addr[2], _buffer->list[i].addr[1],
_buffer->list[i].addr[0], _buffer->list[i].rssiThreshold, _buffer->counters[i]);
}
}
void ScanResult::print() const {
for (int i = 0; i < getSize(); ++i) {
LOGd("[%02X %02X %02X %02X %02X %02X]\trssi: %d\tocc: %d", _buffer->list[i].addr[5],
_buffer->list[i].addr[4], _buffer->list[i].addr[3], _buffer->list[i].addr[2], _buffer->list[i].addr[1],
_buffer->list[i].addr[0], _buffer->list[i].rssi, _buffer->list[i].occurrences);
}
}
void Storage::getUint16(uint32_t value, uint16_t& target, uint16_t default_value) {
#ifdef PRINT_ITEMS
uint8_t* tmp = (uint8_t*)&value;
LOGi("raw value: %02X %02X %02X %02X", tmp[3], tmp[2], tmp[1], tmp[0]);
#endif
// check if last byte is FF which means that memory is unnassigned
// and value has to be ignored
if (value & (0xFF << 3)) {
LOGd("use default value");
target = default_value;
} else {
target = value;
LOGd("found stored value: %d", target);
}
}
void Storage::getUint32(uint32_t value, uint32_t& target, uint32_t default_value) {
#ifdef PRINT_ITEMS
uint8_t* tmp = (uint8_t*)&value;
LOGi("raw value: %02X %02X %02X %02X", tmp[3], tmp[2], tmp[1], tmp[0]);
#endif
// check if value is equal to INT_MAX (FFFFFFFF) which means that memory is
// unnassigned and value has to be ignored
if (value == INT_MAX) {
LOGd("use default value");
target = default_value;
} else {
target = value;
LOGd("found stored value: %d", target);
}
}
int32_t
ferret_fpages_request_component (CORBA_Object _dice_corba_obj,
uint16_t major,
uint16_t minor,
uint16_t instance,
l4_snd_fpage_t *page,
CORBA_Server_Environment *_dice_corba_env)
{
local_object_id_t id;
l4vfs_th_node_t * node;
char path[MAX_PATH];
sensor_entry_t * se;
int ksem;
LOG("entry");
snprintf(path, MAX_PATH, "/%d/%d/%d", major, minor, instance);
id = l4vfs_th_resolve(0, path);
if (id == L4VFS_ILLEGAL_OBJECT_ID)
{
LOGd(verbose, "Request for unknown sensor");
return -1; // not found
}
node = l4vfs_th_node_for_id(id);
se = (sensor_entry_t *)(node->data);
if (se->type != FERRET_ULIST)
{
LOGd(verbose, "Request on wrong sensor type for this operation");
return -2;
}
ksem = (int)se->data;
page->snd_base = 0; // fixme: is this right?
page->fpage.iofp.grant = 0;
page->fpage.iofp.zero1 = 0;
page->fpage.iofp.iosize = 0;
page->fpage.iofp.zero2 = 2;
page->fpage.iofp.iopage = ksem;
page->fpage.iofp.f = 0xf;
LOG("exit");
return 0;
}
/**
* Event handler on receiving a message from
*/
void rbc_mesh_event_handler(rbc_mesh_event_t* evt)
{
TICK_PIN(28);
//nrf_gpio_gitpin_toggle(PIN_GPIO_LED1);
switch (evt->event_type)
{
case RBC_MESH_EVENT_TYPE_CONFLICTING_VAL:
LOGd("conflicting value");
break;
case RBC_MESH_EVENT_TYPE_NEW_VAL:
LOGd("new value");
break;
case RBC_MESH_EVENT_TYPE_UPDATE_VAL:
LOGd("update value");
break;
case RBC_MESH_EVENT_TYPE_INITIALIZED:
LOGd("initialized");
break;
}
switch (evt->event_type)
{
// case RBC_MESH_EVENT_TYPE_CONFLICTING_VAL:
case RBC_MESH_EVENT_TYPE_NEW_VAL:
case RBC_MESH_EVENT_TYPE_UPDATE_VAL: {
if (evt->value_handle > 2)
break;
//if (evt->data[0]) {
LOGi("Got data ch: %i, val: %i, len: %d, orig_addr:", evt->value_handle, evt->data[0], evt->data_len);
// BLEutil::printArray(evt->originator_address.addr, 6);
MeshControl &meshControl = MeshControl::getInstance();
meshControl.process(evt->value_handle, evt->data, evt->data_len);
//}
//led_config(evt->value_handle, evt->data[0]);
break;
}
default:
LOGi("Default: %i", evt->event_type);
break;
}
}
void State::savePersistentStorageItem(uint8_t type) {
#ifdef PRINT_DEBUG
LOGd("store type: %d", type);
#endif
switch (type) {
case STATE_SCHEDULE: {
savePersistentStorageItem(_storageStruct.scheduleList, sizeof(schedule_list_t));
}
}
}
int worker(void *data)
{
struct thread_data *tdata = (struct thread_data *)data;
LOGd("worker %u start.\n", tdata->id);
while (!kthread_should_stop()) {
LOGd("worker %u sleeps.\n", tdata->id);
wait_event_interruptible_timeout(
tdata->wait_q,
test_bit(THREAD_WAKEUP, &tdata->flags) || kthread_should_stop(),
msecs_to_jiffies(tdata->timeout_ms));
clear_bit(THREAD_WAKEUP, &tdata->flags);
LOGd("worker %u woke up.\n", tdata->id);
}
LOGd("worker %u stop.\n", tdata->id);
return 0;
}
int32_t
ferret_monitor_attach_component(CORBA_Object _dice_corba_obj,
uint16_t major,
uint16_t minor,
uint16_t instance,
l4dm_dataspace_t *ds,
CORBA_Server_Environment *_dice_corba_env)
{
local_object_id_t id;
l4vfs_th_node_t * node;
char path[MAX_PATH];
int ret;
if (major == FERRET_TBUF_MAJOR && minor == FERRET_TBUF_MINOR)
{
LOGd(verbose, "Request for trace buffer");
return -2; // not found
}
snprintf(path, MAX_PATH, "/%d/%d/%d", major, minor, instance);
id = l4vfs_th_resolve(0, path);
if (id == L4VFS_ILLEGAL_OBJECT_ID)
{
LOGd(verbose, "Request for unknown sensor");
return -1; // not found
}
node = l4vfs_th_node_for_id(id);
*ds = ((sensor_entry_t *)(node->data))->ds;
LOGd(verbose, "Found id = %d.", id);
ret = l4dm_share(ds, *_dice_corba_obj, L4DM_RO);
if (ret)
{
LOG("Cannot share access rights for ds: ret = %d", ret);
return -1;
}
node->usage_count++;
return 0;
}
// helper function to get std::string from char array, or default value
// if the value read is empty, unassigned (filled with FF) or too long
void Storage::getString(char* value, std::string& target, std::string default_value) {
#ifdef PRINT_ITEMS
_log(INFO, "get string (raw) : ");
for (int i = 0; i < MAX_STRING_SIZE; i++) {
_log(INFO, "%X ", value[i]);
}
_log(INFO, "\r\n");
#endif
target = std::string(value);
// if the last char is equal to FF that means the memory
// is new and has not yet been written to, so we use the
// default value. same if the stored value is an empty string
if (target == "" || value[MAX_STRING_SIZE-1] == 0xFF) {
LOGd("use default value");
target = default_value;
} else {
LOGd("found stored value: %s", target.c_str());
}
}
// helper function to get std::string from char array, or default value
// if the value read is empty, unassigned (filled with FF) or too long
void Storage::getString(char* value, std::string& target, std::string default_value) {
#ifdef PRINT_ITEMS
_log(INFO, "get string (raw): \r\n");
BLEutil::printArray((uint8_t*)value, MAX_STRING_SIZE);
#endif
target = std::string(value);
// if the last char is equal to FF that means the memory
// is new and has not yet been written to, so we use the
// default value. same if the stored value is an empty string
if (target == "" || value[MAX_STRING_SIZE-1] == 0xFF) {
#ifdef PRINT_ITEMS
LOGd("use default value");
#endif
target = default_value;
} else {
#ifdef PRINT_ITEMS
LOGd("found stored value: %s", target.c_str());
#endif
}
}
void IndoorLocalizationService::setRSSILevel(int8_t RSSILevel) {
#ifdef MICRO_VIEW
//! Update rssi at the display
write("2 %i\r\n", RSSILevel);
#endif
if (_rssiCharac) {
*_rssiCharac = RSSILevel;
#ifdef PWM_ON_RSSI
//! avg = 0.4*rssi + (1-0.4)*avg
_averageRssi = (RSSILevel*4 + _averageRssi*6) / 10;
LOGd("RSSI: %d avg: %d", RSSILevel, _averageRssi);
#endif
}
}
void State::setNotify(uint8_t type, bool enable) {
// TODO: why do we support notifications for multiple types at once?
if (enable) {
if (!isNotifying(type)) {
#ifdef PRINT_DEBUG
LOGd("enable notifications for %d", type);
#endif
_notifyingStates.push_back(type);
_notifyingStates.shrink_to_fit();
// LOGd("size: %d", _notifyingStates.size());
}
} else {
std::vector<uint8_t>::iterator it = find(_notifyingStates.begin(), _notifyingStates.end(), type);
if (it != _notifyingStates.end()) {
// if (isNotifying(type)) {
#ifdef PRINT_DEBUG
LOGd("disable notifications for %d", type);
#endif
_notifyingStates.erase(it);
_notifyingStates.shrink_to_fit();
// LOGd("size: %d", _notifyingStates.size());
}
}
}
uint32_t mesh_gatt_value_set(rbc_mesh_value_handle_t handle, uint8_t* data,
uint8_t length) {
if (length > RBC_MESH_VALUE_MAX_LEN) {
return NRF_ERROR_INVALID_LENGTH;
}
if (m_active_conn_handle != CONN_HANDLE_INVALID) {
if (nb_full()) {
LOGw(FMT_BUFFER_FULL, "Notification");
return NRF_ERROR_NO_MEM;
} else {
if (notifactionsPending && !m_mesh_service.notification_enabled) {
notifactionsPending = false;
nb_clear();
}
// todo: continue here: check if we can put on the scheduler
waiting_notification_t* notification = nb_next();
notification->offset = 0;
memcpy(notification->data, data, length);
notification->length = length;
notification->handle = handle;
if (!notifactionsPending) {
// LOGd("put into scheduler");
app_sched_event_put(NULL, 0, value_set_handler);
// printArray(notification, sizeof(waiting_notification_t));
} else {
#ifdef PRINT_MESH_VERBOSE
LOGd("notification pending already");
#endif
return BLE_ERROR_NO_TX_BUFFERS;
}
}
} else {
return BLE_ERROR_INVALID_CONN_HANDLE;
}
}
/** @inherit */
int PowerSamples::assign(buffer_ptr_t buffer, uint16_t size) {
if (getMaxLength() > size) {
LOGe(STR_ERR_BUFFER_NOT_LARGE_ENOUGH);
return 1;
}
#ifdef PRINT_POWERSAMPLES_VERBOSE
LOGd(FMT_ASSIGN_BUFFER_LEN, buffer, size);
#endif
_buffer = (power_samples_t*)buffer;
_currentBuffer.assign((buffer_ptr_t)&_buffer->_currentSamples, sizeof(_buffer->_currentSamples));
_voltageBuffer.assign((buffer_ptr_t)&_buffer->_voltageSamples, sizeof(_buffer->_voltageSamples));
return 0;
}
int32_t
ferret_monitor_list_component(CORBA_Object _dice_corba_obj,
ferret_monitor_list_entry_t **entries,
int32_t *count,
int32_t offset,
CORBA_Server_Environment *_dice_corba_env)
{
LOGd(verbose, "list request with offset = '%d', count = '%d'.",
offset, *count);
//sensors_dump_tree(root, 0);
*count = sensors_fill_list(*entries, offset, *count);
if (*count < 0)
return -1;
else
return 0;
}
bool PowerSamples::init() {
//! Allocate memory
_buffer = (power_samples_t*)calloc(1, sizeof(power_samples_t));
if (_buffer == NULL) {
LOGw(STR_ERR_ALLOCATE_MEMORY);
return false;
}
#ifdef PRINT_POWERSAMPLES_VERBOSE
LOGd(FMT_ALLOCATE_MEMORY, _buffer);
#endif
_allocatedSelf = true;
_currentBuffer.assign((buffer_ptr_t)&_buffer->_currentSamples, sizeof(_buffer->_currentSamples));
_voltageBuffer.assign((buffer_ptr_t)&_buffer->_voltageSamples, sizeof(_buffer->_voltageSamples));
//! Also call clear to make sure we start with a clean buffer
clear();
return true;
}
void value_set_handler(void* p_event_data, uint16_t event_size) {
// LOGd("value_set_handler");
// waiting_notification_t* notification = (waiting_notification_t*) p_event_data;
waiting_notification_t* notification = nb_peek();
// printArray(notification, sizeof(waiting_notification_t));
uint32_t err_code = send_notification(notification);
if (err_code == BLE_ERROR_NO_TX_BUFFERS) {
#ifdef PRINT_MESH_VERBOSE
LOGd("adding pending notification");
#endif
notifactionsPending = true;
} else {
// LOGd("popping notification");
nb_pop();
}
}
