arm_uc_scheduler.c

00001 // ----------------------------------------------------------------------------
00002 // Copyright 2016-2017 ARM Ltd.
00003 //
00004 // SPDX-License-Identifier: Apache-2.0
00005 //
00006 // Licensed under the Apache License, Version 2.0 (the "License");
00007 // you may not use this file except in compliance with the License.
00008 // You may obtain a copy of the License at
00009 //
00010 //     http://www.apache.org/licenses/LICENSE-2.0
00011 //
00012 // Unless required by applicable law or agreed to in writing, software
00013 // distributed under the License is distributed on an "AS IS" BASIS,
00014 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00015 // See the License for the specific language governing permissions and
00016 // limitations under the License.
00017 // ----------------------------------------------------------------------------
00018 
00019 #include "update-client-common/arm_uc_config.h"
00020 #include "update-client-common/arm_uc_scheduler.h"
00021 #include "update-client-common/arm_uc_trace.h"
00022 #include "update-client-common/arm_uc_error.h"
00023 
00024 #include "atomic-queue/atomic-queue.h"
00025 
00026 static struct atomic_queue arm_uc_queue = { 0 };
00027 static void (*arm_uc_notificationHandler)(void) = NULL;
00028 static volatile int32_t arm_uc_queue_counter = 0;
00029 
00030 int32_t ARM_UC_SchedulerGetQueuedCount(void) {
00031     return arm_uc_queue_counter;
00032 }
00033 
00034 #if ARM_UC_SCHEDULER_STORAGE_POOL_SIZE
00035 /* Define the scheduler's callback pool storage.
00036  * The scheduler will allocate out of this pool whenever it encounters a
00037  * callback that is already locked or a callback that is NULL.
00038  */
00039 static arm_uc_callback_t callback_pool_storage[ARM_UC_SCHEDULER_STORAGE_POOL_SIZE];
00040 static arm_uc_callback_t* callback_pool_root;
00041 #endif
00042 
00043 static void (*scheduler_error_cb)(uint32_t parameter);
00044 static arm_uc_callback_t callback_pool_exhausted_error_callback = {0};
00045 static arm_uc_callback_t callback_failed_take_error_callback = {0};
00046 
00047 #define POOL_WATERMARK 0xABABABAB
00048 
00049 void ARM_UC_SchedulerInit(void)
00050 {
00051 #if ARM_UC_SCHEDULER_STORAGE_POOL_SIZE
00052     /* Initialize the storage pool */
00053     callback_pool_root = callback_pool_storage;
00054     for (size_t i = 0; i < ARM_UC_SCHEDULER_STORAGE_POOL_SIZE-1; i++)
00055     {
00056         callback_pool_storage[i].next = &callback_pool_storage[i+1];
00057         /* watermark pool elements by setting the lock to POOL_WATERMARK.
00058          * This allows checking of the maximum number of concurrent allocations.
00059          */
00060         callback_pool_storage[i].lock = POOL_WATERMARK;
00061     }
00062     callback_pool_storage[ARM_UC_SCHEDULER_STORAGE_POOL_SIZE-1].next = NULL;
00063     callback_pool_storage[ARM_UC_SCHEDULER_STORAGE_POOL_SIZE-1].lock = POOL_WATERMARK;
00064 #endif
00065     memset(&callback_pool_exhausted_error_callback, 0, sizeof(arm_uc_callback_t));
00066     memset(&callback_failed_take_error_callback, 0, sizeof(arm_uc_callback_t));
00067 }
00068 
00069 /**
00070  * @brief Allocate a block from the pool
00071  * @details Gets a non-null block from the callback pool.
00072  * 
00073  * Theory of operation:
00074  * * callback_pool_alloc starts by fetching the current value of the pool's
00075  *   root. This value should be the next free item in the pool. 
00076  * * If the value is NULL, then there are no elements left in the pool, so 
00077  *   callback_pool_alloc returns NULL.
00078  * * callback_pool_alloc tries to take this element by replacing the root
00079  *   node with the following element. If replacement fails, callback_pool_alloc
00080  *   tries the whole process again. This is repeated until allocation succeeds
00081  *   or the root pointer is NULL.
00082  * 
00083  * @retval NULL the no element was available to allocate
00084  * @retval non-NULL An allocated element
00085  */
00086 static arm_uc_callback_t* callback_pool_alloc()
00087 {
00088     while (true) {
00089         arm_uc_callback_t* prev_free = callback_pool_root;
00090         if (NULL == prev_free){
00091             return NULL;
00092         }
00093         arm_uc_callback_t* new_free = prev_free->next;
00094         
00095         if (aq_atomic_cas_uintptr((uintptr_t*)&callback_pool_root, (uintptr_t)prev_free, (uintptr_t)new_free)) {
00096             return prev_free;
00097         }
00098     }
00099 }
00100 
00101 /**
00102  * @brief Check if the pool owns a block 
00103  * @detail callback_pool_owns() checks whether the pointer supplied exists
00104  * within the callback_pool_storage array. If it does, that means that the pool
00105  * should own the block.
00106  * 
00107  * @param[in] e the element to evaluate for pool ownership
00108  * 
00109  * @retval 1 the pool owns the callback
00110  * @retval 0 the pool does not own the callback
00111  */
00112 
00113 static int callback_pool_owns(arm_uc_callback_t* e)
00114 {
00115     int isGreater = e >= callback_pool_storage;
00116     int isLesser = (uintptr_t)e < ((uintptr_t)callback_pool_storage + sizeof(callback_pool_storage));
00117     return isGreater && isLesser;
00118 }
00119 
00120 /**
00121  * @brief Free a block owned by the pool.
00122  * @details Checks whether the supplied callback is owned by the pool and frees
00123  * it if so. Performs no operation for a callback that is not owned by the pool.
00124  * 
00125  * @param[in] e the element to free
00126  */
00127 static void callback_pool_free(arm_uc_callback_t* e)
00128 {
00129     UC_COMM_TRACE("%s (%p)", __PRETTY_FUNCTION__, e);
00130     if (callback_pool_owns(e)) {
00131         while (true) {
00132             arm_uc_callback_t* prev_free = callback_pool_root;
00133 
00134             e->next = prev_free;
00135             UC_COMM_TRACE("%s inserting r:%p p:%p, e:%p, ", __PRETTY_FUNCTION__, callback_pool_root, prev_free, e);
00136             if (aq_atomic_cas_uintptr((uintptr_t*)&callback_pool_root, (uintptr_t)prev_free, (uintptr_t)e)) {
00137                 break;
00138             }
00139             UC_COMM_TRACE("%s inserting failed", __PRETTY_FUNCTION__);
00140         }
00141     } 
00142 }
00143 
00144 uint32_t ARM_UC_SchedulerGetHighWatermark(void)
00145 {
00146     uint32_t i;
00147     for (i = 0; i < ARM_UC_SCHEDULER_STORAGE_POOL_SIZE; i++)
00148     {
00149         if (callback_pool_storage[i].lock == POOL_WATERMARK) {
00150             break;
00151         }
00152     }
00153     return i;
00154 }
00155 
00156 
00157 void ARM_UC_AddNotificationHandler(void (*handler)(void))
00158 {
00159     arm_uc_notificationHandler = handler;
00160 }
00161 
00162 void ARM_UC_SetSchedulerErrorHandler(void(*handler)(uint32_t))
00163 {
00164     scheduler_error_cb = handler;
00165 }
00166 
00167 bool ARM_UC_PostCallback(arm_uc_callback_t* _storage,
00168                          void (*_callback)(uint32_t),
00169                          uint32_t _parameter)
00170 {
00171     bool success = true;
00172     UC_COMM_TRACE("%s Scheduling %p(%lu) with %p", __PRETTY_FUNCTION__, _callback, _parameter, _storage);
00173 
00174     if (_callback == NULL)
00175     {
00176         return false;
00177     }
00178 
00179     if (_storage)
00180     {
00181         int result = aq_element_take((void *) _storage);
00182         if (result != ATOMIC_QUEUE_SUCCESS)
00183         {
00184 
00185 // NOTE: This may be useful for detecting double-allocation of callbacks on mbed-os too
00186 #if defined(TARGET_IS_PC_LINUX)
00187             /* On Linux, issue an error message if the callback was not added
00188                to the queue. This is dangerous in mbed-os, since writing to the
00189                console from an interrupt context might crash the program. */
00190                 UC_COMM_TRACE("ARM_UC_PostCallback failed to acquire lock on: %p %p; allocating a temporary callback",
00191                                 _storage,
00192                                 _callback);
00193 
00194 #endif
00195             _storage = NULL;
00196         }
00197     }
00198     if (_storage == NULL)
00199     {
00200         _storage = callback_pool_alloc();
00201         if (_storage == NULL)
00202         {
00203             success = false;
00204             /* Handle a failed alloc */
00205 
00206 #ifdef TARGET_IS_PC_LINUX
00207             /* On Linux, issue an error message if the callback was not added
00208                to the queue. This is dangerous in mbed-os, since writing to the
00209                console from an interrupt context might crash the program. */
00210             UC_COMM_ERR_MSG("Failed to allocate a callback block");
00211 #endif
00212             if (scheduler_error_cb)
00213             {
00214                 _storage = &callback_pool_exhausted_error_callback;
00215                 int result = aq_element_take((void *) _storage);
00216                 if (result == ATOMIC_QUEUE_SUCCESS) {
00217                     _parameter = ARM_UC_EQ_ERR_POOL_EXHAUSTED;
00218                     _callback = scheduler_error_cb;
00219                 }
00220                 else 
00221                 {
00222                     _storage = NULL;
00223                 }
00224             }
00225         }
00226         else
00227         {
00228             /* This thread is guaranteed to exclusively own _storage here */
00229             aq_initialize_element((void*) _storage);
00230             int result = aq_element_take((void*) _storage);
00231             if (result != ATOMIC_QUEUE_SUCCESS)
00232             {
00233                 success = false;
00234                 /* This should be impossible */
00235                 UC_COMM_ERR_MSG("Failed to take an allocated a callback block... this should be impossible...");
00236                 if (scheduler_error_cb)
00237                 {
00238                     _storage = &callback_failed_take_error_callback;
00239                     int result = aq_element_take((void *) _storage);
00240                     if (result == ATOMIC_QUEUE_SUCCESS) {
00241                         _parameter = ARM_UC_EQ_ERR_FAILED_TAKE;
00242                         _callback = scheduler_error_cb;
00243                     }
00244                     else
00245                     {
00246                         _storage = NULL;
00247                     }
00248                 }
00249             }
00250         }
00251     }
00252     if (_storage)
00253     {
00254         /* populate callback struct */
00255         _storage->callback = _callback;
00256         _storage->parameter = _parameter;
00257 
00258         UC_COMM_TRACE("%s Queueing %p(%lu) in %p", __PRETTY_FUNCTION__, _callback, _parameter, _storage);
00259 
00260         /* push struct to atomic queue */
00261         int result = aq_push_tail(&arm_uc_queue, (void *) _storage);
00262 
00263         if (result == ATOMIC_QUEUE_SUCCESS)
00264         {
00265             UC_COMM_TRACE("%s Scheduling success!", __PRETTY_FUNCTION__);
00266 
00267             /*  The queue is processed by removing an element first and then
00268                 decrementing the queue counter. This continues until the counter
00269                 reaches 0 (process single callback) or the queue is empty
00270                 (process queue).
00271 
00272                 If the counter is greater than zero at this point, there should
00273                 already be a notification in progress, so no new notification
00274                 is required.
00275 
00276                 If the counter is zero at this point, there is one element in
00277                 the queue, and incrementing the counter will return 1 and which
00278                 triggers a notification.
00279 
00280                 Because the scheduler could run at any time and consume queue
00281                 elements, it's possible for the scheduler to remove the queue
00282                 element before the counter is incremented.
00283 
00284                 Therefore, If the queue is empty at this point, the counter is
00285                 -1 and incrementing the counter will return 0. This does not
00286                 trigger a notification, which is correct since the queue is 
00287                 empty.
00288             */
00289             int32_t count = aq_atomic_inc_int32((int32_t*) &arm_uc_queue_counter, 1);
00290 
00291             /* if notification handler is set, check if this is the first
00292             insertion
00293             */
00294             if ((arm_uc_notificationHandler) && (count == 1))
00295             {
00296                 UC_COMM_TRACE("%s Invoking notify!", __PRETTY_FUNCTION__);
00297 
00298                 /* disable: UC_COMM_TRACE("notify nanostack scheduler"); */
00299                 arm_uc_notificationHandler();
00300             }
00301         }
00302         else
00303         {
00304             success = false;
00305         }
00306     }
00307 
00308     return success;
00309 }
00310 
00311 void ARM_UC_ProcessQueue(void)
00312 {
00313     arm_uc_callback_t* element = (arm_uc_callback_t*) aq_pop_head(&arm_uc_queue);
00314 
00315     while (element != NULL)
00316     {
00317         UC_COMM_TRACE("%s Invoking %p(%lu)", __PRETTY_FUNCTION__, element->callback, element->parameter);
00318         /* Store the callback locally */
00319         void (*callback)(uint32_t) = element->callback;
00320         /* Store the parameter locally */
00321         uint32_t parameter = element->parameter;
00322         /* Release the lock on the element */
00323         UC_COMM_TRACE("%s Releasing %p", __PRETTY_FUNCTION__, element);
00324         aq_element_release((void*) element);
00325         /* Free the element if it was pool allocated */
00326         UC_COMM_TRACE("%s Freeing %p", __PRETTY_FUNCTION__, element);
00327         callback_pool_free((void*) element);
00328 
00329         /* execute callback */
00330         callback(parameter);
00331 
00332         /*  decrement element counter after executing the callback.
00333             otherwise further callbacks posted inside this callback could
00334             trigger notifications eventhough we are still processing the queue.
00335         */
00336         int32_t count = aq_atomic_inc_int32((int32_t*) &arm_uc_queue_counter, -1);
00337 
00338         if (count > 0) {
00339             /* get next element */
00340             element = (arm_uc_callback_t*) aq_pop_head(&arm_uc_queue);
00341         }
00342         else {
00343             element = NULL;
00344         }
00345     }
00346 }
00347 
00348 bool ARM_UC_ProcessSingleCallback(void)
00349 {
00350     /* elements in queue */
00351     int32_t count = 0;
00352 
00353     /* get first element */
00354     arm_uc_callback_t* element = (arm_uc_callback_t*) aq_pop_head(&arm_uc_queue);
00355 
00356     if (element != NULL)
00357     {
00358         UC_COMM_TRACE("%s Invoking %p(%lu)", __PRETTY_FUNCTION__, element->callback, element->parameter);
00359         /* Store the callback locally */
00360         void (*callback)(uint32_t) = element->callback;
00361         /* Store the parameter locally */
00362         uint32_t parameter = element->parameter;
00363         /* Release the lock on the element */
00364         UC_COMM_TRACE("%s Releasing %p", __PRETTY_FUNCTION__, element);
00365         aq_element_release((void*) element);
00366         /* Free the element if it was pool allocated */
00367         UC_COMM_TRACE("%s Freeing %p", __PRETTY_FUNCTION__, element);
00368         callback_pool_free((void*) element);
00369 
00370         /* execute callback */
00371         callback(parameter);
00372 
00373         UC_COMM_TRACE("%s Decrementing callback counter", __PRETTY_FUNCTION__);
00374 
00375         /*  decrement element counter after executing the callback.
00376             otherwise further callbacks posted inside this callback could
00377             trigger notifications eventhough we are still processing the queue.
00378 
00379             when this function returns false, the counter is 0 and there are
00380             either 1 or 0 elements in the queue. if there is 1 element in
00381             the queue, it means the counter hasn't been incremented yet, and
00382             incrmenting it will return 1, which will trigger a notification.
00383         */
00384         count = aq_atomic_inc_int32((int32_t*) &arm_uc_queue_counter, -1);
00385     }
00386 
00387     return (count > 0);
00388 }