0d3d1d5c83
The util folder contains memory management, queue management and misc utilities used by the Link Layer implementation. This will be in time replaced by Zephyr's native functionality. Jira: ZEP-702 Origin: Original Change-Id: Id8602ea41ec44811801dfc582bab244c339eabe3 Signed-off-by: Vinayak Chettimada <vinayak.kariappa.chettimada@nordicsemi.no> Signed-off-by: Carles Cufi <carles.cufi@nordicsemi.no>
169 lines
2.9 KiB
C
169 lines
2.9 KiB
C
/*
|
|
* Copyright (c) 2016 Nordic Semiconductor ASA
|
|
* Copyright (c) 2016 Vinayak Kariappa Chettimada
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
#include <stdint.h>
|
|
#include "hal_irq.h"
|
|
|
|
#include "work.h"
|
|
|
|
static struct work *_work_head;
|
|
|
|
#ifdef __GNUC__
|
|
static inline uint32_t __disable_irq(void)
|
|
{
|
|
uint32_t result;
|
|
|
|
__asm__ volatile ("MRS %0, PRIMASK\n\t CPSID i":"=r" (result));
|
|
|
|
return (result & 0x01);
|
|
}
|
|
|
|
static inline void __enable_irq(void)
|
|
{
|
|
__asm__ volatile ("CPSIE i");
|
|
}
|
|
#endif
|
|
|
|
void work_enable(uint8_t group)
|
|
{
|
|
irq_enable(group);
|
|
}
|
|
|
|
void work_disable(uint8_t group)
|
|
{
|
|
irq_disable(group);
|
|
}
|
|
|
|
uint8_t work_enabled(uint8_t group)
|
|
{
|
|
return irq_enabled(group);
|
|
}
|
|
|
|
uint32_t work_schedule(struct work *w, uint8_t chain)
|
|
{
|
|
int was_masked = __disable_irq();
|
|
struct work *prev;
|
|
struct work *curr;
|
|
|
|
/* Dequeue expired work at head */
|
|
while ((_work_head)
|
|
&& (_work_head->ack == _work_head->req)
|
|
) {
|
|
_work_head = _work_head->next;
|
|
}
|
|
|
|
/* Dequeue expired in between list and find last node */
|
|
curr = _work_head;
|
|
prev = curr;
|
|
while (curr) {
|
|
/* delete expired work */
|
|
if (curr->ack == curr->req) {
|
|
prev->next = curr->next;
|
|
} else {
|
|
prev = curr;
|
|
}
|
|
|
|
curr = curr->next;
|
|
}
|
|
|
|
/* chain, if explicitly requested, or if work not at current level */
|
|
chain = chain || (!irq_priority_equal(w->group))
|
|
|| (!irq_enabled(w->group));
|
|
|
|
/* Already in List */
|
|
curr = _work_head;
|
|
while (curr) {
|
|
if (curr == w) {
|
|
if (!chain) {
|
|
break;
|
|
}
|
|
|
|
if (!was_masked) {
|
|
__enable_irq();
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
curr = curr->next;
|
|
}
|
|
|
|
/* handle work(s) that can be inline */
|
|
if (!chain) {
|
|
w->req = w->ack;
|
|
|
|
if (!was_masked) {
|
|
__enable_irq();
|
|
}
|
|
|
|
if (w->fp) {
|
|
w->fp(w->params);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/* New, add to the list */
|
|
w->req = w->ack + 1;
|
|
w->next = 0;
|
|
if (prev == curr) {
|
|
_work_head = w;
|
|
} else {
|
|
prev->next = w;
|
|
}
|
|
|
|
irq_pending_set(w->group);
|
|
|
|
if (!was_masked) {
|
|
__enable_irq();
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
void work_run(uint8_t group)
|
|
{
|
|
int was_masked = __disable_irq();
|
|
struct work *curr = _work_head;
|
|
|
|
while (curr) {
|
|
if ((curr->group == group) && (curr->ack != curr->req)) {
|
|
curr->ack = curr->req;
|
|
|
|
if (curr->fp) {
|
|
if (curr->next) {
|
|
irq_pending_set(group);
|
|
}
|
|
|
|
if (!was_masked) {
|
|
__enable_irq();
|
|
}
|
|
|
|
curr->fp(curr->params);
|
|
|
|
return;
|
|
}
|
|
}
|
|
|
|
curr = curr->next;
|
|
}
|
|
|
|
if (!was_masked) {
|
|
__enable_irq();
|
|
}
|
|
}
|