// Place data passed from other tasks into send queue void putMsgOnSendQueue(char* data) { sendData.enqueueCount++; // Error simulation code, skip send enqueue if (sendData.enqueueCount % MESSAGE_SKIP_DIV == 1) return; if (sendData.sendQ_LR != 0) { // Check for full queue. If no spaces available, call to remove oldest data. if (uxQueueSpacesAvailable( sendData.sendQ_LR ) == 0) { // If message is not removed from queue, return and signal error if (removeSendQueueData() == 0) { dbgOutputVal(SEND_FULLQUEUE); //stopAll(); return; } } // Send to queue, with at least one vacancy guaranteed for this data if( xQueueSend( sendData.sendQ_LR, (void*) data, portMAX_DELAY) != pdPASS ) { dbgOutputVal(MOTOR_SENDTOSENDQ_FAIL); } } }
static void vInterruptCountingSemaphoreTask( void *pvParameters ) { BaseType_t xCount; const TickType_t xDelay = pdMS_TO_TICKS( intsemINTERRUPT_MUTEX_GIVE_PERIOD_MS ) * ( intsemMAX_COUNT + 1 ); ( void ) pvParameters; for( ;; ) { /* Expect to start with the counting semaphore empty. */ if( uxQueueMessagesWaiting( ( QueueHandle_t ) xISRCountingSemaphore ) != 0 ) { xErrorDetected = pdTRUE; } /* Wait until it is expected that the interrupt will have filled the counting semaphore. */ xOkToGiveCountingSemaphore = pdTRUE; vTaskDelay( xDelay ); xOkToGiveCountingSemaphore = pdFALSE; /* Now it is expected that the counting semaphore is full. */ if( uxQueueMessagesWaiting( ( QueueHandle_t ) xISRCountingSemaphore ) != intsemMAX_COUNT ) { xErrorDetected = pdTRUE; } if( uxQueueSpacesAvailable( ( QueueHandle_t ) xISRCountingSemaphore ) != 0 ) { xErrorDetected = pdTRUE; } ulCountingSemaphoreLoops++; /* Expect to be able to take the counting semaphore intsemMAX_COUNT times. A block time of 0 is used as the semaphore should already be there. */ xCount = 0; while( xSemaphoreTake( xISRCountingSemaphore, 0 ) == pdPASS ) { xCount++; } if( xCount != intsemMAX_COUNT ) { xErrorDetected = pdTRUE; } /* Now raise the priority of this task so it runs immediately that the semaphore is given from the interrupt. */ vTaskPrioritySet( NULL, configMAX_PRIORITIES - 1 ); /* Block to wait for the semaphore to be given from the interrupt. */ xOkToGiveCountingSemaphore = pdTRUE; xSemaphoreTake( xISRCountingSemaphore, portMAX_DELAY ); xSemaphoreTake( xISRCountingSemaphore, portMAX_DELAY ); xOkToGiveCountingSemaphore = pdFALSE; /* Reset the priority so as not to disturbe other tests too much. */ vTaskPrioritySet( NULL, tskIDLE_PRIORITY ); ulCountingSemaphoreLoops++; } }
UBaseType_t MPU_uxQueueSpacesAvailable( const QueueHandle_t xQueue ) { BaseType_t xRunningPrivileged = xPortRaisePrivilege(); UBaseType_t uxReturn; uxReturn = uxQueueSpacesAvailable( xQueue ); vPortResetPrivilege( xRunningPrivileged ); return uxReturn; }
// Check local motor queue for vacancies. If none, remove oldest data. // After, add the parameter char* message to local motor queue. void putDataOnMotorQ(char* data) { if (motorData.motorQ_LR != 0) { // Check for full queue. If no spaces available, call to remove oldest data. if (uxQueueSpacesAvailable( motorData.motorQ_LR ) == 0) { // If message is not removed from queue, return and signal error if (removeMotorQueueData() == 0) { dbgOutputVal(MOTOR_FULLQUEUE); stopAll(); return; } } // Send to queue, with at least one vacancy guaranteed for this data if( xQueueSend( motorData.motorQ_LR, (void*) data, portMAX_DELAY) != pdPASS ) { dbgOutputVal(RECEIVE_SENDTOMOTORQ_FAIL); } } }
// Place data passed from other tasks into send queue void putMsgOnSendQueue(char* data) { if (sendData.sendQ_CD != 0) { // Check for full queue. If no spaces available, call to remove oldest data. if (uxQueueSpacesAvailable( sendData.sendQ_CD ) == 0) { // If message is not removed from queue, return and signal error if (removeSendQueueData() == 0) { dbgOutputVal(SEND_FULLQUEUE); //stopAll(); return; } } // Send to queue, with at least one vacancy guaranteed for this data if( xQueueSend( sendData.sendQ_CD, (void*) data, portMAX_DELAY) != pdPASS ) { dbgOutputVal(MOTOR_SENDTOSENDQ_FAIL); } } }
BaseType_t FreeRTOS_FD_SET( xSocket_t xSocket, xSocketSet_t xSocketSet ) { xFreeRTOS_Socket_t *pxSocket = ( xFreeRTOS_Socket_t * ) xSocket; BaseType_t xReturn = pdFALSE; UBaseType_t uxMessagesWaiting; configASSERT( xSocket ); /* Is the socket already a member of a select group? */ if( pxSocket->xSelectQueue == NULL ) { taskENTER_CRITICAL(); { /* Are there packets queued on the socket already? */ uxMessagesWaiting = uxQueueMessagesWaiting( pxSocket->xWaitingPacketSemaphore ); /* Are there enough notification spaces in the select queue for the number of packets already queued on the socket? */ if( uxQueueSpacesAvailable( ( xQueueHandle ) xSocketSet ) >= uxMessagesWaiting ) { /* Store a pointer to the select group in the socket for future reference. */ pxSocket->xSelectQueue = ( xQueueHandle ) xSocketSet; while( uxMessagesWaiting > 0 ) { /* Add notifications of the number of packets that are already queued on the socket to the select queue. */ xQueueSendFromISR( pxSocket->xSelectQueue, &pxSocket, NULL ); uxMessagesWaiting--; } xReturn = pdPASS; } } taskEXIT_CRITICAL(); } return xReturn; }
static void prvDemoQueueSpaceFunctions( void *pvParameters ) { QueueHandle_t xQueue = NULL; const unsigned portBASE_TYPE uxQueueLength = 10; unsigned portBASE_TYPE uxReturn, x; /* Remove compiler warnings. */ ( void ) pvParameters; /* Create the queue that will be used. Nothing is actually going to be sent or received so the queue item size is set to 0. */ xQueue = xQueueCreate( uxQueueLength, 0 ); configASSERT( xQueue ); for( ;; ) { for( x = 0; x < uxQueueLength; x++ ) { /* Ask how many messages are available... */ uxReturn = uxQueueMessagesWaiting( xQueue ); /* Check the number of messages being reported as being available is as expected, and force an assert if not. */ if( uxReturn != x ) { /* xQueue cannot be NULL so this is deliberately causing an assert to be triggered as there is an error. */ configASSERT( xQueue == NULL ); } /* Ask how many spaces remain in the queue... */ uxReturn = uxQueueSpacesAvailable( xQueue ); /* Check the number of spaces being reported as being available is as expected, and force an assert if not. */ if( uxReturn != ( uxQueueLength - x ) ) { /* xQueue cannot be NULL so this is deliberately causing an assert to be triggered as there is an error. */ configASSERT( xQueue == NULL ); } /* Fill one more space in the queue. */ xQueueSendToBack( xQueue, NULL, 0 ); } /* Perform the same check while the queue is full. */ uxReturn = uxQueueMessagesWaiting( xQueue ); if( uxReturn != uxQueueLength ) { configASSERT( xQueue == NULL ); } uxReturn = uxQueueSpacesAvailable( xQueue ); if( uxReturn != 0 ) { configASSERT( xQueue == NULL ); } /* The queue is full, start again. */ xQueueReset( xQueue ); #if( configUSE_PREEMPTION == 0 ) taskYIELD(); #endif } }
/** * \brief BLE manager task */ static void ble_mgr_task(void *pvParameters) { ad_ble_hdr_t *msg_rx; uint32_t ulNotifiedValue; OS_BASE_TYPE xResult; int8_t wdog_id; /* Register task to be monitored by watch dog. */ wdog_id = sys_watchdog_register(false); for (;;) { /* Notify watch dog on each loop since there's no other trigger for this. */ sys_watchdog_notify(wdog_id); /* Suspend monitoring while task is blocked on OS_TASK_NOTIFY_WAIT(). */ sys_watchdog_suspend(wdog_id); /* * Wait on any of the event group bits, then clear them all. */ xResult = OS_TASK_NOTIFY_WAIT(0x0, OS_TASK_NOTIFY_ALL_BITS, &ulNotifiedValue, OS_TASK_NOTIFY_FOREVER); OS_ASSERT(xResult == OS_OK); /* Resume watch dog monitoring. */ sys_watchdog_notify_and_resume(wdog_id); if (ulNotifiedValue & mainBIT_ADAPTER_EVENT_QUEUE) { /* Make sure there are messages waiting on the queue. */ if (!uxQueueMessagesWaiting(adapter_if->evt_q)) { goto no_event; } /* Check if there is free space on BLE manager's event queue. */ if (uxQueueSpacesAvailable(mgr_if.evt_q)) { /* Get message from queue. */ OS_QUEUE_GET(adapter_if->evt_q, &msg_rx, 0); OS_ASSERT(msg_rx->op_code < AD_BLE_OP_CODE_LAST); #ifdef BLE_STACK_PASSTHROUGH_MODE { OS_IRB new_irb; /* Fill-in new IRB fields. */ new_irb.status = IRB_PENDING; new_irb.class_id = IRB_BLE; new_irb.ptr_buf = msg_rx; /* Send directly to BLE manager's event queue. */ ble_mgr_event_queue_send(&new_irb, OS_QUEUE_FOREVER); } #else if (msg_rx->op_code == AD_BLE_OP_CODE_STACK_MSG) { irb_ble_stack_msg_t *stack_msg = (irb_ble_stack_msg_t*) msg_rx; /* In non-passthrough we only expect GTL messages. */ OS_ASSERT(stack_msg->msg_type == GTL_MSG); /* * during reset we ignore messages other than GAPM_CMP_EVT * and GAPM_RESET operation */ if (reset) { struct gapm_cmp_evt *evt; if (stack_msg->msg.gtl.msg_id != GAPM_CMP_EVT) { goto rx_done; } evt = (void *) stack_msg->msg.gtl.param; if (evt->operation != GAPM_RESET) { goto rx_done; } } /* * Check if someone is waiting for this message. * if not, try to handle message as an event. */ if (!ble_gtl_waitqueue_match(&stack_msg->msg.gtl)) { if (!ble_gtl_handle_event(&stack_msg->msg.gtl)) { /* Stack message is not handled by the manager. */ #ifdef DEBUG configASSERT(0); #endif } } } else if (msg_rx->op_code == AD_BLE_OP_CODE_ADAPTER_MSG) { ad_ble_msg_t *ad_msg = (ad_ble_msg_t *) msg_rx; /* In non-passthrough we only expect GTL messages. */ OS_ASSERT(ad_msg->operation < AD_BLE_OP_LAST); /* Check if someone is waiting for this message. */ ble_ad_msg_waitqueue_match(ad_msg); } rx_done: OS_FREE(msg_rx); #endif /* * Check if there are more messages waiting in the BLE adapter's * event queue. */ if (uxQueueMessagesWaiting(adapter_if->evt_q)) { OS_TASK_NOTIFY(mgr_if.task, mainBIT_ADAPTER_EVENT_QUEUE, OS_NOTIFY_SET_BITS); } } else { /* Set blocked flag to true. */ ble_mgr_blocked = true; } } no_event: if (ulNotifiedValue & mainBIT_MANAGER_COMMAND_QUEUE) { if (uxQueueMessagesWaiting(mgr_if.cmd_q)) { OS_IRB irb_rx; /* Get IRB from the queue. */ OS_QUEUE_GET(mgr_if.cmd_q, &irb_rx, 0); if (irb_rx.status == IRB_COMPLETED) { /* Free message buffer if it was not freed by application. */ irb_ble_free_msg(&irb_rx); } else if (irb_rx.status == IRB_ERROR) { ble_mgr_event_queue_send(&irb_rx, OS_QUEUE_FOREVER); } else if (irb_rx.status == IRB_PENDING) { /* New IRB from application. */ if (!ble_irb_handle_msg(&irb_rx)) { /* * No handler found for IRB - free command buffer * because nothing else will free it. */ OS_FREE(irb_rx.ptr_buf); irb_rx.ptr_buf = NULL; } } /* Check if blocked and if there is space on the event queue. */ if (ble_mgr_blocked && uxQueueSpacesAvailable(mgr_if.evt_q)) { /* Set flag to false. */ ble_mgr_blocked = false; /* Notify task to resume getting BLE adapter events. */ OS_TASK_NOTIFY(mgr_if.task, mainBIT_ADAPTER_EVENT_QUEUE, OS_NOTIFY_SET_BITS); } /* Check if there are messages waiting in the command queue. */ if (uxQueueMessagesWaiting(mgr_if.cmd_q)) { OS_TASK_NOTIFY(mgr_if.task, mainBIT_MANAGER_COMMAND_QUEUE, OS_NOTIFY_SET_BITS); } } } /* * Check this bit as last one since previous commands may also update storage. In * such case changes will be written to flash already and there's no need to execute * this twice in a row. */ if (ulNotifiedValue & mainBIT_COMMIT_STORAGE) { /* * To commit anything modified in storage it's enough to acquire and then * immediately release lock - if dirty flag was set, contents of storage * will be written to flash automatically. */ storage_acquire(); storage_release(); } /* Check if BLE adapter is blocked and if there is free space on its event queue. */ if (ble_mgr_adapter_is_blocked() && uxQueueSpacesAvailable(adapter_if->evt_q)) { /* Notify BLE adapter that there is free space on its event queue. */ ad_ble_notify_event_queue_avail(); } } }