/*      $NetBSD: softintr.c,v 1.18 2020/11/21 21:27:09 thorpej Exp $    */

/*-
 * Copyright (c) 2001 The NetBSD Foundation, Inc.
 * All rights reserved.
 *
 * This code is derived from software contributed to The NetBSD Foundation
 * by IWAMOTO Toshihiro.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <sys/cdefs.h>
__KERNEL_RCSID(0, "$NetBSD: softintr.c,v 1.18 2020/11/21 21:27:09 thorpej Exp $");

#include <sys/param.h>
#include <sys/systm.h>
#include <sys/syslog.h>
#include <sys/kmem.h>

#include <machine/cpu.h>
#include <arm/cpufunc.h>
#include <machine/intr.h>

void softintr_free(void *);
void softintr_dispatch(int);

struct softintr_handler {
        struct softintr_handler *sh_vlink;      /* vertical link */
        struct softintr_handler *sh_hlink;      /* horizontal link */

        void (*sh_fun)(void *);
        void *sh_arg;
        int sh_level;

        int sh_pending;
};

struct softintr_handler *softintrs;
struct softintr_handler *softintr_pending;

void *
softintr_establish(int level, void (*fun)(void *), void *arg)
{
        struct softintr_handler *sh;

        sh = kmem_alloc(sizeof(*sh), KM_SLEEP);
        sh->sh_fun = fun;
        sh->sh_level = ipl_to_spl(level);
        sh->sh_arg = arg;
        sh->sh_pending = 0;

        return sh;
}

void
softintr_disestablish(void *cookie)
{
        struct softintr_handler *sh = cookie;
        u_int saved_cpsr;

        saved_cpsr = SetCPSR(I32_bit, I32_bit);
        if (sh->sh_pending) {
                sh->sh_fun = softintr_free;
                sh->sh_arg = sh;
                SetCPSR(I32_bit, I32_bit & saved_cpsr);
        } else {
                SetCPSR(I32_bit, I32_bit & saved_cpsr);
                kmem_free(sh, sizeof(*sh));
        }
}

void
softintr_free(void *arg)
{
        struct softintr_handler *sh = arg;

        kmem_free(sh, sizeof(*sh));
}

void
softintr_schedule(void *cookie)
{
        struct softintr_handler **p, *sh = cookie;
        register int pending, saved_cpsr;

        pending = 1;
        __asm("swp %0, %0, [%1]" : "+r" (pending) : "r" (&sh->sh_pending));

        if (pending)
                return;

        sh->sh_vlink = NULL;
        sh->sh_hlink = NULL;

#ifdef __GNUC__
        __asm volatile("mrs %0, cpsr\n orr r1, %0, %1\n msr cpsr_a;;, r1" :
            "=r" (saved_cpsr) : "i" (I32_bit) : "r1");
#else
        saved_cpsr = SetCPSR(I32_bit, I32_bit);
#endif
        p = &softintr_pending;

        for (;; p = &(*p)->sh_vlink) {
                if (*p == NULL)
                        goto set_and_exit;

                if ((*p)->sh_level <= sh->sh_level)
                        break;
        }

        if ((*p)->sh_level == sh->sh_level) {
                sh->sh_hlink = *p;
                sh->sh_vlink = (*p)->sh_vlink;
                goto set_and_exit;
        }

        sh->sh_vlink = *p;
set_and_exit:
        *p = sh;
#ifdef __GNUC__
        __asm volatile("msr cpsr_c, %0" : : "r" (saved_cpsr));
#else
        SetCPSR(I32_bit, I32_bit & saved_cpsr);
#endif
        return;
}

void
softintr_dispatch(int s)
{
        struct softintr_handler *sh, *sh1;
        register int saved_cpsr;

        while (1) {
                /* Protect list operation from interrupts */
#ifdef __GNUC__
                __asm volatile("mrs %0, cpsr\n orr r1, %0, %1\n"
                    " msr cpsr_all, r1" : "=r" (saved_cpsr) :
                    "i" (I32_bit) : "r1");
#else
                saved_cpsr = SetCPSR(I32_bit, I32_bit);
#endif

                if (softintr_pending == NULL ||
                    softintr_pending->sh_level <= s) {
#ifdef __GNUC__
                        __asm("msr cpsr_c, %0" : : "r" (saved_cpsr));
#else
                        SetCPSR(I32_bit, I32_bit & saved_cpsr);
#endif
                        splx(s);
                        return;
                }
                sh = softintr_pending;
                softintr_pending = softintr_pending->sh_vlink;

                if (sh->sh_level > current_spl_level)
                        raisespl(sh->sh_level);
#ifdef __GNUC__
                __asm volatile("msr cpsr_c, %0" : : "r" (saved_cpsr));
#else
                SetCPSR(I32_bit, I32_bit & saved_cpsr);
#endif
                if (sh->sh_level < current_spl_level)
                        lowerspl(sh->sh_level);

                while (1) {
                        /* The order is important */
                        sh1 = sh->sh_hlink;
                        sh->sh_pending = 0;

                        (sh->sh_fun)(sh->sh_arg);
                        if (sh1 == NULL)
                                break;
                        sh = sh1;
                }
        }
        splx(s);
}