Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

hrt_ioctl: Fix issue with SMP #867

Closed
wants to merge 1 commit into from
Closed

hrt_ioctl: Fix issue with SMP #867

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 9 additions & 26 deletions platforms/nuttx/src/px4/common/hrt_ioctl.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,21 +68,6 @@ static sq_queue_t callout_inflight;

static spinlock_t g_hrt_ioctl_lock = SP_UNLOCKED;

/* Check if entry is in list */

static bool entry_inlist(sq_queue_t *queue, sq_entry_t *item)
{
sq_entry_t *queued;

sq_for_every(queue, queued) {
if (queued == item) {
return true;
}
}

return false;
}

/* Find (pop) first entry for user from queue, the queue must be locked prior */

static struct usr_hrt_call *pop_user(sq_queue_t *queue, const px4_hrt_handle_t handle)
Expand Down Expand Up @@ -143,16 +128,13 @@ static struct usr_hrt_call *dup_entry(const px4_hrt_handle_t handle, struct hrt_
e = (void *)sq_remfirst(&callout_freelist);
}

spin_unlock_irqrestore_wo_note(&g_hrt_ioctl_lock, flags);

if (!e) {
/* Allocate a new kernel side item for the user call */

e = kmm_malloc(sizeof(struct usr_hrt_call));
}

if (e) {

/* Store the user side callout function and argument to the user's handle */
entry->callout = callout;
entry->arg = arg;
Expand All @@ -165,28 +147,29 @@ static struct usr_hrt_call *dup_entry(const px4_hrt_handle_t handle, struct hrt_
e->usr_entry = entry;

/* Add this to the callout_queue list */
flags = spin_lock_irqsave_wo_note(&g_hrt_ioctl_lock);
sq_addfirst(&e->list_item, &callout_queue);
spin_unlock_irqrestore_wo_note(&g_hrt_ioctl_lock, flags);

sq_addfirst(&e->list_item, &callout_queue);
} else {
PX4_ERR("out of memory");

}

spin_unlock_irqrestore_wo_note(&g_hrt_ioctl_lock, flags);

return e;
}

void hrt_usr_call(void *arg)
{
// This is called from hrt interrupt
struct usr_hrt_call *e = (struct usr_hrt_call *)arg;
irqstate_t flags = spin_lock_irqsave_wo_note(&g_hrt_ioctl_lock);

// Make sure the event is not already in flight
if (!entry_inlist(&callout_inflight, (sq_entry_t *)e)) {
sq_addfirst(&e->list_item, &callout_inflight);
px4_sem_post(e->entry.callout_sem);
}
sq_addfirst(&e->list_item, &callout_inflight);

spin_unlock_irqrestore_wo_note(&g_hrt_ioctl_lock, flags);

px4_sem_post(e->entry.callout_sem);
}

int hrt_ioctl(unsigned int cmd, unsigned long arg);
Expand Down
Loading