path: root/io-module/radio_udev_discovery.c

#include <linux/types.h>
#include <linux/errno.h>
#include <linux/platform_device.h>
#include <linux/pid.h>
#include <linux/version.h>
#if LINUX_VERSION_CODE >= KERNEL_VERSION(4,11,0)
#include <linux/signalfd.h>
#endif
#include "mts_io_module.h"
#include "version.h"
#include "radio_udev_discovery.h"

/*
 *  radio-reset in the mts-io driver sets radio_udev_discovery to 0.
 *  Then it returns early if radio_udev_discovery is set to 1.  The
 *  UDEV daemon is used to set radio_udev_discovery.  Notifications
 *  are sent if radio_udev_discovery goes from 0 to 1.
 */

int radio_udev_discovery = 1;


struct sig_pid_radio_reset_monitor_s {
    pid_t pid;
    int signal;
    struct pid *vpid;
};

#define PID_MAX_COUNT 20
static struct sig_pid_radio_reset_monitor_s sig_pid_radio_reset_monitor[PID_MAX_COUNT];


/* Clear an entry in the table that no longer exists.
 * unlocks the mts_io_lock, toggles the rcu_read_lock,
 * then puts back the mts_io_lock.
 */
static int
verify_vpid(struct sig_pid_radio_reset_monitor_s *p)
{
    pid_t pid0 = p->pid;
    struct pid *vpid1;
    struct pid *vpid0 = p->vpid;
    unsigned int vcount;

    // Check to see if vpid and pid still match
    if (p->pid == 0)
        return 0;
    if (vpid0 == NULL) {
        p->pid = 0;
        return 0;
    }

    mutex_unlock(&mts_io_mutex);
    rcu_read_lock();
    vpid1 = find_vpid((pid_t)pid0);
    if(vpid1 != NULL) {
        #if LINUX_VERSION_CODE >= KERNEL_VERSION(4,15,0)
            vcount = refcount_read(&vpid1->count);
        #else
            vcount = atomic_read(&vpid1->count);
        #endif
    } else
        vcount = 0;
    rcu_read_unlock();
    mutex_lock(&mts_io_mutex);
    if ((vpid1 != vpid0) || (vpid1 == NULL))
        p->pid = 0;
    pr_debug("%s: verify_vpid: vpid0 0x%x vpid1 0x%x vcount %d for pid %d\n",
        __FUNCTION__,
        (unsigned int)vpid0, (unsigned int)vpid1, vcount, (int)pid0);
    return p->pid;
}

// Need to find existing entries and allow updates. Signal 0 removes a value.
ssize_t mts_attr_store_radio_reset_monitor(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
{
    long pid0;
    int sig;
    int i;
    int found;
    struct pid *vpid1, *vpid0;
    int retval;

    retval = sscanf(buf, "%ld %d", &pid0, &sig);
    if (retval != 2)
        return -EINVAL;

    if (pid0 < 0)
	return -EINVAL;

    rcu_read_lock();
    vpid0 = find_vpid((pid_t)pid0);
    rcu_read_unlock();
    pr_debug("%s: Try to store %ld %d 0x%x\n",
        __FUNCTION__,
        pid0,sig,(unsigned int)vpid0);
    if (vpid0) {
        /* Note that since we no longer hold the rcu lock, vpid
         * could become invalid, unless it is ours.
         */
        mutex_lock(&mts_io_mutex);
        /* First see if we are already in the table -- search whole table */
        found = -1;
        for (i=0; i < PID_MAX_COUNT; i++) {
            if ((sig_pid_radio_reset_monitor[i].pid == (pid_t)pid0) &&
                    (sig_pid_radio_reset_monitor[i].vpid == vpid0)) {
                if (sig == 0) {
                    sig_pid_radio_reset_monitor[i].pid = 0;
                    sig_pid_radio_reset_monitor[i].vpid = NULL;
		} else {
                    sig_pid_radio_reset_monitor[i].signal = sig;
                    mutex_unlock(&mts_io_mutex);
                    rcu_read_lock();
                    vpid0 = find_vpid((pid_t)pid0);
                    rcu_read_unlock();
                    pr_debug("%s: slot %d: pid %d, vpid 0x%x/vpid 0x%x\n",
                        __FUNCTION__,
                        i,(int)pid0,(unsigned int)vpid0,
			(unsigned int)sig_pid_radio_reset_monitor[i].vpid);
                    mutex_lock(&mts_io_mutex);
                    if (vpid0 != sig_pid_radio_reset_monitor[i].vpid) {
                        sig_pid_radio_reset_monitor[i].pid = 0; /* Maybe exited? */
                        sig_pid_radio_reset_monitor[i].vpid = NULL;
                        pr_debug("%s: mts-io: vpid  mismatch pid %d, vpid 0x%x/0x%x, sig %d, cleared %d\n",
                                __FUNCTION__,
				(int)sig_pid_radio_reset_monitor[i].pid,
				(unsigned int)sig_pid_radio_reset_monitor[i].vpid,(unsigned int)vpid0,
				(unsigned int)sig_pid_radio_reset_monitor[i].signal,i);
                    } else {
                        found = i;
                        pr_debug("%s: mts-io: found pid %d, vpid 0x%x, sig %d\n",
                                __FUNCTION__,
				(int)sig_pid_radio_reset_monitor[i].pid,
				(unsigned int)sig_pid_radio_reset_monitor[i].vpid,
				(unsigned int)sig_pid_radio_reset_monitor[i].signal);
                        sig = 0;  // Clear any more that we find.
                    }

                }
            }  // Location matches our pid
        } // Loop through table
        
        pr_debug("%s: found=%d, sig=%d\n",__FUNCTION__,found,sig);
        if (found > -1) {
            mutex_unlock(&mts_io_mutex);
            return count;
        }

        if (sig == 0) {
            // Nothing to clear out.
            mutex_unlock(&mts_io_mutex);
            return count;
        }

        /* Need to find an unused slot and save our signal and PID.
         * Still holding mutex_lock(&mts_io_mutex)
         * Find first open slot
         */
        found = -1;
        for (i=0; i < PID_MAX_COUNT; i++) {
            if (sig_pid_radio_reset_monitor[i].pid == 0) {
                sig_pid_radio_reset_monitor[i].pid = (pid_t)pid0;
                sig_pid_radio_reset_monitor[i].signal = sig;
                sig_pid_radio_reset_monitor[i].vpid = vpid0;
                pr_debug("%s: mts-io: open slot: pid %d, vpid 0x%x, sig %d, slot %d\n",
                        __FUNCTION__,
			(int)pid0,
			(unsigned int)vpid0,
			(unsigned int)sig, i);
                found = i;
                break;
            } else {
		// See if this slot may be used.
		pid_t pid1 = sig_pid_radio_reset_monitor[i].pid;
                if (pid1 > 0) {
                    mutex_unlock(&mts_io_mutex);
                    rcu_read_lock();
                    vpid1 = find_vpid((pid_t)pid1);
                    rcu_read_unlock();
                    mutex_lock(&mts_io_mutex);
                    // See if pid and vpid still exists
                    if (!vpid1 || (vpid1 != sig_pid_radio_reset_monitor[i].vpid)) {
                        // Make sure no one else has claimed this slot
                        if (pid1 == sig_pid_radio_reset_monitor[i].pid) {
                            sig_pid_radio_reset_monitor[i].pid = 0;
                            sig_pid_radio_reset_monitor[i].vpid = NULL;
                            i--;  // Should be able to use this slot.
                            continue;
                        } // Has not been re-claimed.
                    } // Slot has no user or exited user
                }  // Verify that slot in use is still in use
            }  // Found an empty slot
        }  // Loop through all the slots.
        mutex_unlock(&mts_io_mutex);
    } // pid is in pid table
    return count;
}

// Examples say buf is PAGE_SIZE long
ssize_t mts_attr_show_radio_reset_monitor(struct device *dev,
			struct device_attribute *attr,
			char *buf)
{
        int count;
        int max = PAGE_SIZE;
        int ret;
        int i;
        pid_t pid1;

        mutex_lock(&mts_io_mutex);

        count = 0;
        for (i=0; i < PID_MAX_COUNT; i++) {
            pid1 = verify_vpid(sig_pid_radio_reset_monitor + i);
            if (pid1 == 0) {
                sig_pid_radio_reset_monitor[i].pid = 0;
                sig_pid_radio_reset_monitor[i].vpid = NULL;
                continue;
            }
            pr_debug("%s: found a pid in slot %d\n",__FUNCTION__,i);
            ret = snprintf(buf+count, max, "%lu %d\n",
                           (unsigned long)pid1,
                           sig_pid_radio_reset_monitor[i].signal);
            if (ret > 0) {
                max -= ret;
                count += ret;
            }
            if (max == 0)
                break;
            if(ret == 0)
                break;
        }  // Loop through all the slots until we are full.

	mutex_unlock(&mts_io_mutex);

	return count;
}


/*
 * After a radio reset, prime the flag, so if it is set to one
 * after this, then we wake the processes waiting on the flag.
 * Caller holds the mts_io_mutex lock.
 */
void
reset_radio_udev_discovery(void)
{
    radio_udev_discovery = 0;
}

int
udev_discovered_radio(void)
{
    return radio_udev_discovery;
}

ssize_t mts_attr_show_radio_udev_discovery(struct device *dev,
			struct device_attribute *attr,
			char *buf)
{
    return sprintf(buf, "%d\n", radio_udev_discovery);
}


ssize_t mts_attr_store_radio_udev_discovery(struct device *dev,
		struct device_attribute *attr, const char *buf, size_t count)
{
    int value = -1;
    pid_t pid1;
    struct pid *vpid0, *vpid1;
    int sig, i;
    int retval;

    retval = sscanf(buf, "%i", &value);
    if (retval != 1)
        return -EINVAL;

    if (radio_udev_discovery == -1) {
        // Not ready
        return -EPROTO;
    }

    // Only driver can set this to zero.
    if (value == 0)
        return -EINVAL;

    mutex_lock(&mts_io_mutex);
    if((value == 1) && (radio_udev_discovery == 0)) {
        radio_udev_discovery = 1;
        pr_debug("%s: mts-io: UDEV discovered cellular modem after radio-reset so signal registered processes",
                 __FUNCTION__);
        // kill all processes that are queued
        for (i=0; i < PID_MAX_COUNT; i++) {
            pid1 = verify_vpid(sig_pid_radio_reset_monitor + i);
            if (pid1 == 0) continue;
            sig = sig_pid_radio_reset_monitor[i].signal;
            vpid0 = sig_pid_radio_reset_monitor[i].vpid;
            mutex_unlock(&mts_io_mutex);
            rcu_read_lock();
            vpid1 = find_vpid(pid1);
            if (vpid1 == vpid0) {
                printk(KERN_ALERT
                    "mts-io: %s: Sending signal %d to pid %d due to UDEV radio discovery\n",
                    __FUNCTION__,
                    sig,pid1);
                kill_pid(vpid1,sig,1);
            }  // vpid is valid, same as registered
              else {
                printk(KERN_ALERT
                    "mts-io: %s: mismatched vpid %x/%x\n", __FUNCTION__, (unsigned int)vpid1, (unsigned int) vpid0);
            }

            rcu_read_unlock();
            mutex_lock(&mts_io_mutex);
        }  // Find empty slot
    }  // udev wants us to wake everybody up after reset
    mutex_unlock(&mts_io_mutex);
    return count;
}