diff --git a/src/bacnet/basic/service/h_cov.c b/src/bacnet/basic/service/h_cov.c index 6ef0c7c8..a1b54a5d 100644 --- a/src/bacnet/basic/service/h_cov.c +++ b/src/bacnet/basic/service/h_cov.c @@ -6,6 +6,7 @@ * @copyright SPDX-License-Identifier: MIT */ #include +#include #include #include #include @@ -28,6 +29,7 @@ #include "bacnet/basic/tsm/tsm.h" #include "bacnet/basic/object/device.h" #include "bacnet/basic/services.h" +#include "bacnet/basic/sys/keylist.h" #include "bacnet/basic/sys/debug.h" #include "bacnet/datalink/datalink.h" @@ -43,7 +45,6 @@ typedef struct BACnet_COV_Address { /* note: This COV service only monitors the properties of an object that have been specified in the standard. */ typedef struct BACnet_COV_Subscription_Flags { - bool valid : 1; bool issueConfirmedNotifications : 1; /* optional */ bool send_requested : 1; } BACNET_COV_SUBSCRIPTION_FLAGS; @@ -57,12 +58,11 @@ typedef struct BACnet_COV_Subscription { BACNET_OBJECT_ID monitoredObjectIdentifier; } BACNET_COV_SUBSCRIPTION; -#ifndef MAX_COV_SUBCRIPTIONS -#define MAX_COV_SUBCRIPTIONS 128 +#ifndef MAX_COV_SUBSCRIPTIONS +#define MAX_COV_SUBSCRIPTIONS 128 #endif -static BACNET_COV_SUBSCRIPTION COV_Subscriptions_List[MAX_NUM_DEVICES] - [MAX_COV_SUBCRIPTIONS]; +static OS_Keylist COV_Subscriptions_List[MAX_NUM_DEVICES]; #ifdef BAC_ROUTING #define COV_Subscriptions (COV_Subscriptions_List[Routed_Device_Object_Index()]) #else @@ -73,6 +73,53 @@ static BACNET_COV_SUBSCRIPTION COV_Subscriptions_List[MAX_NUM_DEVICES] #endif static BACNET_COV_ADDRESS COV_Addresses[MAX_COV_ADDRESSES]; +/** + * @brief Deletes a subscription + * @param list_idx - keylist index + * @return true if the subscription was deleted + */ +static bool cov_subscription_delete(uint32_t list_idx) +{ + bool status = false; + BACNET_COV_SUBSCRIPTION *subscription = NULL; + + subscription = Keylist_Data_Delete_By_Index(COV_Subscriptions, list_idx); + if (subscription) { + free(subscription); + status = true; + } + + return status; +} + +/** + * @brief Creates a subscription + * @param list_key - keylist key + * @return the subscription that was created, or NULL + */ +static BACNET_COV_SUBSCRIPTION *cov_subscription_create(uint32_t list_key) +{ + BACNET_COV_SUBSCRIPTION *subscription = NULL; + int index = 0; + if (list_key >= MAX_COV_SUBSCRIPTIONS) { + return NULL; + } + subscription = Keylist_Data(COV_Subscriptions, list_key); + if (!subscription) { + subscription = calloc(1, sizeof(BACNET_COV_SUBSCRIPTION)); + if (subscription) { + index = Keylist_Data_Add(COV_Subscriptions, list_key, subscription); + if (index < 0) { + free(subscription); + return NULL; + } + } else { + return NULL; + } + } + return subscription; +} + /** * Gets the address from the list of COV addresses * @@ -103,6 +150,7 @@ static void cov_address_remove_unused(void) unsigned index = 0; unsigned cov_index = 0; bool found = false; + BACNET_COV_SUBSCRIPTION *subscription = NULL; #ifdef BAC_ROUTING uint16_t current_dev_id = Routed_Device_Object_Index(); @@ -112,9 +160,11 @@ static void cov_address_remove_unused(void) found = false; for (dev_id = 0; dev_id < Get_Num_Managed_Devices(); dev_id++) { Set_Routed_Device_Object_Index(dev_id); - for (index = 0; index < MAX_COV_SUBCRIPTIONS; index++) { - if ((COV_Subscriptions[index].flag.valid) && - (COV_Subscriptions[index].dest_index == cov_index)) { + for (index = 0; index < Keylist_Count(COV_Subscriptions); + index++) { + subscription = Keylist_Data_Index(COV_Subscriptions, index); + if (subscription && + (subscription->dest_index == cov_index)) { found = true; break; } @@ -133,9 +183,9 @@ static void cov_address_remove_unused(void) for (cov_index = 0; cov_index < MAX_COV_ADDRESSES; cov_index++) { if (COV_Addresses[cov_index].valid) { found = false; - for (index = 0; index < MAX_COV_SUBCRIPTIONS; index++) { - if ((COV_Subscriptions[index].flag.valid) && - (COV_Subscriptions[index].dest_index == cov_index)) { + for (index = 0; index < Keylist_Count(COV_Subscriptions); index++) { + subscription = Keylist_Data_Index(COV_Subscriptions, index); + if (subscription && (subscription->dest_index == cov_index)) { found = true; break; } @@ -323,14 +373,15 @@ int handler_cov_encode_subscriptions(uint8_t *apdu, int max_apdu) }; unsigned index = 0; int apdu_len = 0; + BACNET_COV_SUBSCRIPTION *subscription = NULL; - for (index = 0; index < MAX_COV_SUBCRIPTIONS; index++) { - if (COV_Subscriptions[index].flag.valid) { + for (index = 0; index < Keylist_Count(COV_Subscriptions); index++) { + subscription = Keylist_Data_Index(COV_Subscriptions, index); + if (subscription) { /* Lets encode a COV subscription into an intermediate buffer * that can hold it */ int len = cov_encode_subscription( - &cov_sub[0], max_apdu - apdu_len, - &COV_Subscriptions[index]); + &cov_sub[0], max_apdu - apdu_len, subscription); if ((apdu_len + len) > max_apdu) { return -2; @@ -359,34 +410,18 @@ void handler_cov_init(void) uint16_t dev_id = 0; for (dev_id = 0; dev_id < MAX_NUM_DEVICES; dev_id++) { Set_Routed_Device_Object_Index(dev_id); - for (index = 0; index < MAX_COV_SUBCRIPTIONS; index++) { - /* initialize with invalid COV address */ - COV_Subscriptions[index].flag.valid = false; - COV_Subscriptions[index].dest_index = MAX_COV_ADDRESSES; - COV_Subscriptions[index].subscriberProcessIdentifier = 0; - COV_Subscriptions[index].monitoredObjectIdentifier.type = - OBJECT_ANALOG_INPUT; - COV_Subscriptions[index].monitoredObjectIdentifier.instance = 0; - COV_Subscriptions[index].flag.issueConfirmedNotifications = false; - COV_Subscriptions[index].invokeID = 0; - COV_Subscriptions[index].lifetime = 0; - COV_Subscriptions[index].flag.send_requested = false; + if (!COV_Subscriptions) { + COV_Subscriptions = Keylist_Create(); + } else { + Keylist_Data_Free(COV_Subscriptions); } } Set_Routed_Device_Object_Index(current_dev_id); #else - for (index = 0; index < MAX_COV_SUBCRIPTIONS; index++) { - /* initialize with invalid COV address */ - COV_Subscriptions[index].flag.valid = false; - COV_Subscriptions[index].dest_index = MAX_COV_ADDRESSES; - COV_Subscriptions[index].subscriberProcessIdentifier = 0; - COV_Subscriptions[index].monitoredObjectIdentifier.type = - OBJECT_ANALOG_INPUT; - COV_Subscriptions[index].monitoredObjectIdentifier.instance = 0; - COV_Subscriptions[index].flag.issueConfirmedNotifications = false; - COV_Subscriptions[index].invokeID = 0; - COV_Subscriptions[index].lifetime = 0; - COV_Subscriptions[index].flag.send_requested = false; + if (!COV_Subscriptions) { + COV_Subscriptions = Keylist_Create(); + } else { + Keylist_Data_Free(COV_Subscriptions); } #endif for (index = 0; index < MAX_COV_ADDRESSES; index++) { @@ -402,57 +437,54 @@ static bool cov_list_subscribe( { bool existing_entry = false; int index; - int first_invalid_index = -1; bool found = true; bool address_match = false; const BACNET_ADDRESS *dest = NULL; + BACNET_COV_SUBSCRIPTION *subscription = NULL; /* unable to subscribe - resources? */ /* unable to cancel subscription - other? */ /* existing? - match Object ID and Process ID and address */ - for (index = 0; index < MAX_COV_SUBCRIPTIONS; index++) { - if (COV_Subscriptions[index].flag.valid) { - dest = cov_address_get(COV_Subscriptions[index].dest_index); + for (index = 0; index < Keylist_Count(COV_Subscriptions); index++) { + subscription = Keylist_Data_Index(COV_Subscriptions, index); + if (subscription) { + dest = cov_address_get(subscription->dest_index); if (dest) { address_match = bacnet_address_same(src, dest); } else { /* skip address matching - we don't have an address */ address_match = true; } - if ((COV_Subscriptions[index].monitoredObjectIdentifier.type == + if ((subscription->monitoredObjectIdentifier.type == cov_data->monitoredObjectIdentifier.type) && - (COV_Subscriptions[index].monitoredObjectIdentifier.instance == + (subscription->monitoredObjectIdentifier.instance == cov_data->monitoredObjectIdentifier.instance) && - (COV_Subscriptions[index].subscriberProcessIdentifier == + (subscription->subscriberProcessIdentifier == cov_data->subscriberProcessIdentifier) && address_match) { existing_entry = true; + if (subscription->invokeID) { + tsm_free_invoke_id(subscription->invokeID); + subscription->invokeID = 0; + } if (cov_data->cancellationRequest) { - /* initialize with invalid COV address */ - COV_Subscriptions[index].flag.valid = false; - COV_Subscriptions[index].dest_index = MAX_COV_ADDRESSES; + cov_subscription_delete(index); cov_address_remove_unused(); } else { - COV_Subscriptions[index].dest_index = cov_address_add(src); - COV_Subscriptions[index].flag.issueConfirmedNotifications = + subscription->dest_index = cov_address_add(src); + subscription->flag.issueConfirmedNotifications = cov_data->issueConfirmedNotifications; - COV_Subscriptions[index].lifetime = cov_data->lifetime; - COV_Subscriptions[index].flag.send_requested = true; - } - if (COV_Subscriptions[index].invokeID) { - tsm_free_invoke_id(COV_Subscriptions[index].invokeID); - COV_Subscriptions[index].invokeID = 0; + subscription->lifetime = cov_data->lifetime; + subscription->flag.send_requested = true; } + break; } - } else { - if (first_invalid_index < 0) { - first_invalid_index = index; - } } } - if (!existing_entry && (first_invalid_index >= 0) && + if (!existing_entry && + (Keylist_Count(COV_Subscriptions) < MAX_COV_SUBSCRIPTIONS) && (!cov_data->cancellationRequest)) { const int addr_add_ret = cov_address_add(src); @@ -461,24 +493,30 @@ static bool cov_list_subscribe( *error_code = ERROR_CODE_NO_SPACE_TO_ADD_LIST_ELEMENT; found = false; } else { - index = first_invalid_index; found = true; - COV_Subscriptions[index].dest_index = addr_add_ret; - COV_Subscriptions[index].flag.valid = true; - COV_Subscriptions[index].monitoredObjectIdentifier.type = - cov_data->monitoredObjectIdentifier.type; - COV_Subscriptions[index].monitoredObjectIdentifier.instance = - cov_data->monitoredObjectIdentifier.instance; - COV_Subscriptions[index].subscriberProcessIdentifier = - cov_data->subscriberProcessIdentifier; - COV_Subscriptions[index].flag.issueConfirmedNotifications = - cov_data->issueConfirmedNotifications; - COV_Subscriptions[index].invokeID = 0; - COV_Subscriptions[index].lifetime = cov_data->lifetime; - COV_Subscriptions[index].flag.send_requested = true; + index = Keylist_Next_Empty_Key(COV_Subscriptions, 0); + subscription = cov_subscription_create(index); + if (subscription) { + subscription->dest_index = addr_add_ret; + subscription->monitoredObjectIdentifier.type = + cov_data->monitoredObjectIdentifier.type; + subscription->monitoredObjectIdentifier.instance = + cov_data->monitoredObjectIdentifier.instance; + subscription->subscriberProcessIdentifier = + cov_data->subscriberProcessIdentifier; + subscription->flag.issueConfirmedNotifications = + cov_data->issueConfirmedNotifications; + subscription->invokeID = 0; + subscription->lifetime = cov_data->lifetime; + subscription->flag.send_requested = true; + } else { + *error_class = ERROR_CLASS_RESOURCES; + *error_code = ERROR_CODE_NO_SPACE_TO_ADD_LIST_ELEMENT; + found = false; + } } } else if (!existing_entry) { - if (first_invalid_index < 0) { + if (Keylist_Count(COV_Subscriptions) >= MAX_COV_SUBSCRIPTIONS) { /* Out of resources */ *error_class = ERROR_CLASS_RESOURCES; *error_code = ERROR_CODE_NO_SPACE_TO_ADD_LIST_ELEMENT; @@ -581,38 +619,38 @@ COV_FAILED: static void cov_lifetime_expiration_handler( unsigned index, uint32_t elapsed_seconds, uint32_t lifetime_seconds) { - if (index < MAX_COV_SUBCRIPTIONS) { + if (index < MAX_COV_SUBSCRIPTIONS) { + BACNET_COV_SUBSCRIPTION *subscription = + Keylist_Data_Index(COV_Subscriptions, index); /* handle lifetime expiration */ if (lifetime_seconds >= elapsed_seconds) { - COV_Subscriptions[index].lifetime -= elapsed_seconds; + subscription->lifetime -= elapsed_seconds; #if 0 fprintf(stderr, "COVtimer: subscription[%d].lifetime=%lu\n", index, - (unsigned long) COV_Subscriptions[index].lifetime); + (unsigned long) subscription->lifetime); #endif } else { - COV_Subscriptions[index].lifetime = 0; + subscription->lifetime = 0; } - if (COV_Subscriptions[index].lifetime == 0) { + if (subscription->lifetime == 0) { /* expire the subscription */ #if PRINT_ENABLED debug_fprintf( stderr, "COVtimer: PID=%u %s %u time remaining=%u seconds\n", - COV_Subscriptions[index].subscriberProcessIdentifier, + subscription->subscriberProcessIdentifier, bactext_object_type_name( - COV_Subscriptions[index].monitoredObjectIdentifier.type), - COV_Subscriptions[index].monitoredObjectIdentifier.instance, - COV_Subscriptions[index].lifetime); + subscription->monitoredObjectIdentifier.type), + subscription->monitoredObjectIdentifier.instance, + subscription->lifetime); #endif - /* initialize with invalid COV address */ - COV_Subscriptions[index].flag.valid = false; - COV_Subscriptions[index].dest_index = MAX_COV_ADDRESSES; - cov_address_remove_unused(); - if (COV_Subscriptions[index].flag.issueConfirmedNotifications) { - if (COV_Subscriptions[index].invokeID) { - tsm_free_invoke_id(COV_Subscriptions[index].invokeID); - COV_Subscriptions[index].invokeID = 0; + if (subscription->flag.issueConfirmedNotifications) { + if (subscription->invokeID) { + tsm_free_invoke_id(subscription->invokeID); + subscription->invokeID = 0; } } + cov_subscription_delete(index); + cov_address_remove_unused(); } } } @@ -640,18 +678,24 @@ static void cov_lifetime_expiration_handler( */ void handler_cov_timer_seconds(uint32_t elapsed_seconds) { - unsigned index = 0; + int index = 0; uint32_t lifetime_seconds = 0; + BACNET_COV_SUBSCRIPTION *subscription = NULL; + int list_cnt = 0; #ifdef BAC_ROUTING uint16_t current_dev_id = Routed_Device_Object_Index(); uint16_t dev_id = 0; + if (elapsed_seconds) { /* handle the subscription timeouts */ for (dev_id = 0; dev_id < Get_Num_Managed_Devices(); dev_id++) { Set_Routed_Device_Object_Index(dev_id); - for (index = 0; index < MAX_COV_SUBCRIPTIONS; index++) { - if (COV_Subscriptions[index].flag.valid) { - lifetime_seconds = COV_Subscriptions[index].lifetime; + list_cnt = Keylist_Count(COV_Subscriptions); + /* Iterate in reverse order due to subscription deletion */ + for (index = list_cnt - 1; index >= 0; index--) { + subscription = Keylist_Data_Index(COV_Subscriptions, index); + if (subscription) { + lifetime_seconds = subscription->lifetime; if (lifetime_seconds) { /* only expire COV with definite lifetimes */ cov_lifetime_expiration_handler( @@ -665,9 +709,12 @@ void handler_cov_timer_seconds(uint32_t elapsed_seconds) #else if (elapsed_seconds) { /* handle the subscription timeouts */ - for (index = 0; index < MAX_COV_SUBCRIPTIONS; index++) { - if (COV_Subscriptions[index].flag.valid) { - lifetime_seconds = COV_Subscriptions[index].lifetime; + list_cnt = Keylist_Count(COV_Subscriptions); + /* Iterate in reverse order due to subscription deletion */ + for (index = list_cnt - 1; index >= 0; index--) { + subscription = Keylist_Data_Index(COV_Subscriptions, index); + if (subscription) { + lifetime_seconds = subscription->lifetime; if (lifetime_seconds) { /* only expire COV with definite lifetimes */ cov_lifetime_expiration_handler( @@ -708,81 +755,81 @@ bool handler_cov_fsm(void) int index = indices[dev_id]; cov_fsm_state_t cov_task_state = cov_task_states[dev_id]; + BACNET_COV_SUBSCRIPTION *subscription = + Keylist_Data_Index(COV_Subscriptions, index); + int list_cnt = Keylist_Count(COV_Subscriptions); switch (cov_task_state) { case COV_STATE_IDLE: - if (COV_Subscriptions[index].flag.valid) { + if (subscription) { cov_task_state = COV_STATE_MARK; } else { index++; - if (index >= MAX_COV_SUBCRIPTIONS) { + if (index >= list_cnt) { index = 0; } } break; case COV_STATE_MARK: /* mark any subscriptions where the value has changed */ - if (COV_Subscriptions[index].flag.valid) { - object_type = (BACNET_OBJECT_TYPE)COV_Subscriptions[index] - .monitoredObjectIdentifier.type; + if (subscription) { + object_type = (BACNET_OBJECT_TYPE) + subscription->monitoredObjectIdentifier.type; object_instance = - COV_Subscriptions[index].monitoredObjectIdentifier.instance; + subscription->monitoredObjectIdentifier.instance; status = Device_COV(object_type, object_instance); if (status) { - COV_Subscriptions[index].flag.send_requested = true; + subscription->flag.send_requested = true; #if PRINT_ENABLED debug_fprintf(stderr, "COVtask: Marking...\n"); #endif } } index++; - if (index >= MAX_COV_SUBCRIPTIONS) { + if (index >= list_cnt) { index = 0; cov_task_state = COV_STATE_CLEAR; } break; case COV_STATE_CLEAR: /* clear the COV flag after checking all subscriptions */ - if ((COV_Subscriptions[index].flag.valid) && - (COV_Subscriptions[index].flag.send_requested)) { - object_type = (BACNET_OBJECT_TYPE)COV_Subscriptions[index] - .monitoredObjectIdentifier.type; + if (subscription && (subscription->flag.send_requested)) { + object_type = (BACNET_OBJECT_TYPE) + subscription->monitoredObjectIdentifier.type; object_instance = - COV_Subscriptions[index].monitoredObjectIdentifier.instance; + subscription->monitoredObjectIdentifier.instance; Device_COV_Clear(object_type, object_instance); } index++; - if (index >= MAX_COV_SUBCRIPTIONS) { + if (index >= list_cnt) { index = 0; cov_task_state = COV_STATE_FREE; } break; case COV_STATE_FREE: /* confirmed notification house keeping */ - if ((COV_Subscriptions[index].flag.valid) && - (COV_Subscriptions[index].flag.issueConfirmedNotifications) && - (COV_Subscriptions[index].invokeID)) { - if (tsm_invoke_id_free(COV_Subscriptions[index].invokeID)) { - COV_Subscriptions[index].invokeID = 0; - } else if (tsm_invoke_id_failed( - COV_Subscriptions[index].invokeID)) { - tsm_free_invoke_id(COV_Subscriptions[index].invokeID); - COV_Subscriptions[index].invokeID = 0; + if (subscription && + (subscription->flag.issueConfirmedNotifications) && + (subscription->invokeID)) { + if (tsm_invoke_id_free(subscription->invokeID)) { + subscription->invokeID = 0; + } else if (tsm_invoke_id_failed(subscription->invokeID)) { + tsm_free_invoke_id(subscription->invokeID); + subscription->invokeID = 0; } } index++; - if (index >= MAX_COV_SUBCRIPTIONS) { + if (index >= list_cnt) { index = 0; cov_task_state = COV_STATE_SEND; } break; case COV_STATE_SEND: /* send any COVs that are requested */ - if ((COV_Subscriptions[index].flag.valid) && - (COV_Subscriptions[index].flag.send_requested)) { + if (subscription && (subscription->flag.send_requested)) { send = true; - if (COV_Subscriptions[index].flag.issueConfirmedNotifications) { - if (COV_Subscriptions[index].invokeID != 0) { + if (subscription->flag.issueConfirmedNotifications) { + if (subscription->invokeID != 0) { /* already sending */ send = false; } @@ -792,10 +839,11 @@ bool handler_cov_fsm(void) } } if (send) { - object_type = (BACNET_OBJECT_TYPE)COV_Subscriptions[index] - .monitoredObjectIdentifier.type; - object_instance = COV_Subscriptions[index] - .monitoredObjectIdentifier.instance; + object_type = + (BACNET_OBJECT_TYPE) + subscription->monitoredObjectIdentifier.type; + object_instance = + subscription->monitoredObjectIdentifier.instance; #if PRINT_ENABLED debug_fprintf(stderr, "COVtask: Sending...\n"); #endif @@ -805,16 +853,15 @@ bool handler_cov_fsm(void) status = Device_Encode_Value_List( object_type, object_instance, &value_list[0]); if (status) { - status = cov_send_request( - &COV_Subscriptions[index], &value_list[0]); + status = cov_send_request(subscription, &value_list[0]); } if (status) { - COV_Subscriptions[index].flag.send_requested = false; + subscription->flag.send_requested = false; } } } index++; - if (index >= MAX_COV_SUBCRIPTIONS) { + if (index >= list_cnt) { index = 0; cov_task_state = COV_STATE_IDLE; } diff --git a/src/bacnet/basic/sys/keylist.c b/src/bacnet/basic/sys/keylist.c index 96aba8f6..3dc602e5 100644 --- a/src/bacnet/basic/sys/keylist.c +++ b/src/bacnet/basic/sys/keylist.c @@ -69,7 +69,7 @@ static bool CheckArraySize(OS_Keylist list) /* See if we got the memory we wanted */ if (!new_array) { - return true; + return false; } /* copy the nodes from the old array to the new array */ @@ -171,6 +171,11 @@ int Keylist_Data_Add(OS_Keylist list, KEY key, void *data) int i; /* counts through the array */ if (list && CheckArraySize(list)) { + node = NodeCreate(); + if (!node) { + return -1; + } + /* figure out where to put the new node */ if (list->count) { (void)FindIndex(list, key, &index); @@ -190,14 +195,11 @@ int Keylist_Data_Add(OS_Keylist list, KEY key, void *data) index = 0; } - /* create and add the node */ - node = NodeCreate(); - if (node) { - list->count++; - node->key = key; - node->data = data; - list->array[index] = node; - } + /* add the node */ + list->count++; + node->key = key; + node->data = data; + list->array[index] = node; } return index; }