void k_queue_insert(struct k_queue *queue, void *prev, void *data)
{
struct k_thread *first_pending_thread;
unsigned int key;
key = irq_lock();
first_pending_thread = _unpend_first_thread(&queue->wait_q);
if (first_pending_thread) {
prepare_thread_to_run(first_pending_thread, data);
if (!_is_in_isr() && _must_switch_threads()) {
(void)_Swap(key);
return;
}
} else {
sys_slist_insert(&queue->data_q, prev, data);
if (handle_poll_event(queue)) {
(void)_Swap(key);
return;
}
}
irq_unlock(key);
}
void main(void)
{
int status = TC_FAIL;
TC_PRINT("Starting slist test\n");
TC_PRINT(" - Initializing the list\n");
sys_slist_init(&test_list);
if (!verify_emptyness(&test_list)) {
TC_ERROR("*** test_list should be empty\n");
goto end;
}
TC_PRINT(" - Appending node 1\n");
sys_slist_append(&test_list, &test_node_1);
if (!verify_content_amount(&test_list, 1)) {
TC_ERROR("*** test_list has wrong content\n");
goto end;
}
if (!verify_tail_head(&test_list, &test_node_1, &test_node_1, true)) {
TC_ERROR("*** test_list head/tail are wrong\n");
goto end;
}
TC_PRINT(" - Finding and removing node 1\n");
sys_slist_find_and_remove(&test_list, &test_node_1);
if (!verify_emptyness(&test_list)) {
TC_ERROR("*** test_list should be empty\n");
goto end;
}
TC_PRINT(" - Prepending node 1\n");
sys_slist_prepend(&test_list, &test_node_1);
if (!verify_content_amount(&test_list, 1)) {
TC_ERROR("*** test_list has wrong content\n");
goto end;
}
if (!verify_tail_head(&test_list, &test_node_1, &test_node_1, true)) {
TC_ERROR("*** test_list head/tail are wrong\n");
goto end;
}
TC_PRINT(" - Removing node 1\n");
sys_slist_remove(&test_list, NULL, &test_node_1);
if (!verify_emptyness(&test_list)) {
TC_ERROR("*** test_list should be empty\n");
goto end;
}
TC_PRINT(" - Appending node 1\n");
sys_slist_append(&test_list, &test_node_1);
TC_PRINT(" - Prepending node 2\n");
sys_slist_prepend(&test_list, &test_node_2);
if (!verify_content_amount(&test_list, 2)) {
TC_ERROR("*** test_list has wrong content\n");
goto end;
}
if (!verify_tail_head(&test_list, &test_node_2, &test_node_1, false)) {
TC_ERROR("*** test_list head/tail are wrong\n");
goto end;
}
TC_PRINT(" - Appending node 3\n");
sys_slist_append(&test_list, &test_node_3);
if (!verify_content_amount(&test_list, 3)) {
TC_ERROR("*** test_list has wrong content\n");
goto end;
}
if (!verify_tail_head(&test_list, &test_node_2, &test_node_3, false)) {
TC_ERROR("*** test_list head/tail are wrong\n");
goto end;
}
if (sys_slist_peek_next(&test_node_2) != &test_node_1) {
TC_ERROR("*** test_list node links are wrong\n");
goto end;
}
TC_PRINT(" - Inserting node 4 after node 2\n");
sys_slist_insert(&test_list, &test_node_2, &test_node_4);
if (!verify_tail_head(&test_list, &test_node_2, &test_node_3, false)) {
TC_ERROR("*** test_list head/tail are wrong\n");
goto end;
}
if (sys_slist_peek_next(&test_node_2) != &test_node_4) {
TC_ERROR("*** test_list node links are wrong\n");
goto end;
}
TC_PRINT(" - Finding and removing node 1\n");
sys_slist_find_and_remove(&test_list, &test_node_1);
if (!verify_content_amount(&test_list, 3)) {
TC_ERROR("*** test_list has wrong content\n");
goto end;
}
if (!verify_tail_head(&test_list, &test_node_2, &test_node_3, false)) {
TC_ERROR("*** test_list head/tail are wrong\n");
goto end;
}
TC_PRINT(" - Removing node 3\n");
sys_slist_remove(&test_list, &test_node_4, &test_node_3);
if (!verify_content_amount(&test_list, 2)) {
TC_ERROR("*** test_list has wrong content\n");
goto end;
}
if (!verify_tail_head(&test_list, &test_node_2, &test_node_4, false)) {
TC_ERROR("*** test_list head/tail are wrong\n");
goto end;
}
TC_PRINT(" - Removing node 4\n");
sys_slist_remove(&test_list, &test_node_2, &test_node_4);
if (!verify_content_amount(&test_list, 1)) {
TC_ERROR("*** test_list has wrong content\n");
goto end;
}
if (!verify_tail_head(&test_list, &test_node_2, &test_node_2, true)) {
TC_ERROR("*** test_list head/tail are wrong\n");
goto end;
}
TC_PRINT(" - Removing node 2\n");
sys_slist_remove(&test_list, NULL, &test_node_2);
if (!verify_emptyness(&test_list)) {
TC_ERROR("*** test_list should be empty\n");
goto end;
}
status = TC_PASS;
end:
TC_END_RESULT(status);
TC_END_REPORT(status);
}
