diff options
Diffstat (limited to 'arch/arm/mach-imx/vf610_sema4.c')
-rw-r--r-- | arch/arm/mach-imx/vf610_sema4.c | 314 |
1 files changed, 314 insertions, 0 deletions
diff --git a/arch/arm/mach-imx/vf610_sema4.c b/arch/arm/mach-imx/vf610_sema4.c new file mode 100644 index 000000000000..9b4d7a74a8ab --- /dev/null +++ b/arch/arm/mach-imx/vf610_sema4.c @@ -0,0 +1,314 @@ +#include <linux/kernel.h> +#include <linux/types.h> +#include <linux/module.h> +#include <linux/platform_device.h> +#include <linux/slab.h> +#include <linux/interrupt.h> +#include <linux/io.h> +#include <linux/device.h> +#include <linux/sched.h> + +#include <linux/mm.h> +#include <linux/version.h> +#include <linux/kdev_t.h> +#include <linux/fs.h> +#include <linux/cdev.h> +#include "hardware.h" +#include <linux/string.h> +#include <linux/uaccess.h> +#include <linux/delay.h> +#include <linux/time.h> +#include <asm/io.h> +#include <linux/debugfs.h> +#include <linux/of_irq.h> +#include <linux/of_address.h> + +#include <linux/vf610_sema4.h> +// ************************************ Local Data ************************************************* + +#define NUM_GATES 16 +#define MASK_FROM_GATE(gate_num) ((u32)(1 << (NUM_GATES*2 - 1 - idx[gate_num]))) + +#define THIS_CORE (0) +#define LOCK_VALUE (THIS_CORE + 1) + +#define SEMA4_CP0INE (0x40) +#define SEMA4_CP0NTF (0x80) + +static MVF_SEMA4* gates[NUM_GATES]; +static void __iomem *sema4_base; + +// account for the way the bits are set / returned in CP0INE and CP0NTF +static const int idx[16] = {3, 2, 1, 0, 7, 6, 5, 4, 11, 10, 9, 8, 15, 14, 13, 12}; + +// debugfs +#define DEBUGFS_DIR "mvf_sema4" +static struct dentry *debugfs_dir; + +// ************************************ Interrupt handler ************************************************* + +static irqreturn_t sema4_irq_handler(int irq, void *dev_id) +{ + int gate_num; + + u32 cp0ntf = readl(sema4_base + SEMA4_CP0NTF); + + for(gate_num=0; gate_num<NUM_GATES; gate_num++) + { + // interrupt from this gate? + if(cp0ntf & MASK_FROM_GATE(gate_num)) + { + // grab the gate to stop the interrupts + writeb(LOCK_VALUE, sema4_base + gate_num); + + // make sure there's a gate assigned + if(gates[gate_num]) + { + //if(gates[gate_num]->use_interrupts) { + // wake up whoever was aiting + wake_up_interruptible(&(gates[gate_num]->wait_queue)); + // bump stats + gates[gate_num]->interrupts++; + //} + } + } + } + + return IRQ_HANDLED; +} + +// ************************************ Utility functions ************************************************* + +int mvf_sema4_assign(int gate_num, MVF_SEMA4** sema4_p) +{ + u32 cp0ine; + unsigned long irq_flags; + char debugfs_gatedir_name[4]; + struct dentry *debugfs_gate_dir; + + if((gate_num < 0) || (gate_num >= NUM_GATES)) + return -EINVAL; + + if(gates[gate_num]) + return -EBUSY; + + *sema4_p = (MVF_SEMA4 *)kmalloc(sizeof(MVF_SEMA4), GFP_KERNEL); + if(*sema4_p == NULL) + return -ENOMEM; + memset(*sema4_p, 0, sizeof(MVF_SEMA4)); + + gates[gate_num] = *sema4_p; + (*sema4_p)->gate_num = gate_num; + + init_waitqueue_head(&((*sema4_p)->wait_queue)); + local_irq_save(irq_flags); + cp0ine = readl(sema4_base + SEMA4_CP0INE); + cp0ine |= MASK_FROM_GATE(gate_num); + writel(cp0ine, sema4_base + SEMA4_CP0INE); + local_irq_restore(irq_flags); + + // debugfs + sprintf(debugfs_gatedir_name, "%d", gate_num); + debugfs_gate_dir = debugfs_create_dir(debugfs_gatedir_name, debugfs_dir); + debugfs_create_u32("attempts", S_IRUGO | S_IWUGO, debugfs_gate_dir, &(*sema4_p)->attempts); + debugfs_create_u32("interrupts", S_IRUGO | S_IWUGO, debugfs_gate_dir, &(*sema4_p)->interrupts); + debugfs_create_u32("failures", S_IRUGO | S_IWUGO, debugfs_gate_dir, &(*sema4_p)->failures); + debugfs_create_u64("total_latency_us", S_IRUGO | S_IWUGO, debugfs_gate_dir, &(*sema4_p)->total_latency_us); + debugfs_create_u32("worst_latency_us", S_IRUGO | S_IWUGO, debugfs_gate_dir, &(*sema4_p)->worst_latency_us); + + return 0; +} +EXPORT_SYMBOL(mvf_sema4_assign); + +int mvf_sema4_deassign(MVF_SEMA4 *sema4) +{ + u32 cp0ine; + unsigned long irq_flags; + + int gate_num; + if(!sema4) + return -EINVAL; + gate_num = sema4->gate_num; + + local_irq_save(irq_flags); + cp0ine = readl(sema4_base + SEMA4_CP0INE); + cp0ine &= ~MASK_FROM_GATE(gate_num); + writel(cp0ine, sema4_base + SEMA4_CP0INE); + local_irq_restore(irq_flags); + + kfree(sema4); + gates[gate_num] = NULL; + + return 0; +} +EXPORT_SYMBOL(mvf_sema4_deassign); + +static long delta_time(struct timeval *start) { + + struct timeval now; + long now_us, start_us; + + do_gettimeofday(&now); + + now_us = (now.tv_sec * 1000000) + now.tv_usec; + start_us = (start->tv_sec * 1000000) + start->tv_usec; + + return now_us > start_us ? now_us - start_us : 0; +} + +static void add_latency_stat(MVF_SEMA4 *sema4) { + + long latency = delta_time(&sema4->request_time); + + sema4->total_latency_us += latency; + sema4->worst_latency_us = sema4->worst_latency_us < latency ? latency : sema4->worst_latency_us; +} + +int mvf_sema4_lock(MVF_SEMA4 *sema4, unsigned int timeout_us, bool use_interrupts) +{ + int retval; + int gate_num; + if(!sema4) + return -EINVAL; + gate_num = sema4->gate_num; + + // bump stats + gates[gate_num]->attempts++; + do_gettimeofday(&gates[gate_num]->request_time); + + // try to grab it + writeb(LOCK_VALUE, sema4_base + gate_num); + if(readb(sema4_base + gate_num) == LOCK_VALUE) { + add_latency_stat(gates[gate_num]); + return 0; + } + + // no timeout, fail + if(!timeout_us) { + gates[gate_num]->failures++; + return -EBUSY; + } + + // spin lock? + if(!use_interrupts) { + while(readb(sema4_base + gate_num) != LOCK_VALUE) { + + if((timeout_us != 0xffffffff) && (delta_time(&gates[gate_num]->request_time) > timeout_us)) { + gates[gate_num]->failures++; + return -EBUSY; + } + + writeb(LOCK_VALUE, sema4_base + gate_num); + } + add_latency_stat(gates[gate_num]); + return 0; + } + + // wait forever? + if(timeout_us == 0xffffffff) + { + if(wait_event_interruptible(sema4->wait_queue, (readb(sema4_base + gate_num) == LOCK_VALUE))) { + gates[gate_num]->failures++; + return -ERESTARTSYS; + } + } + else + { + // return: 0 = timeout, >0 = woke up with that many jiffies left, <0 = error + retval = wait_event_interruptible_timeout(sema4->wait_queue, + (readb(sema4_base + gate_num) == LOCK_VALUE), + usecs_to_jiffies(timeout_us)); + if(retval == 0) { + gates[gate_num]->failures++; + return -ETIME; + } + else if(retval < 0) { + gates[gate_num]->failures++; + return retval; + } + } + + add_latency_stat(gates[gate_num]); + return 0; +} +EXPORT_SYMBOL(mvf_sema4_lock); + +int mvf_sema4_unlock(MVF_SEMA4 *sema4) +{ + if(!sema4) + return -EINVAL; + + // unlock it + writeb(0, sema4_base + sema4->gate_num); + + return 0; +} +EXPORT_SYMBOL(mvf_sema4_unlock); + +// return 0 on success (meaning it is set to us) +int mvf_sema4_test(MVF_SEMA4 *sema4) +{ + if(!sema4) + return -EINVAL; + + return (readb(sema4_base + sema4->gate_num)) == LOCK_VALUE ? 0 : 1; +} +EXPORT_SYMBOL(mvf_sema4_test); + +static int vf610_sema4_probe(struct platform_device *pdev) +{ + struct device *dev = &pdev->dev; + struct resource *iores; + int irq, ret, i; + + iores = platform_get_resource(pdev, IORESOURCE_MEM, 0); + sema4_base = devm_ioremap_resource(dev, iores); + if (IS_ERR(sema4_base)) + return PTR_ERR(sema4_base); + + irq = platform_get_irq(pdev, 0); + if (irq < 0) { + dev_err(&pdev->dev, "no irq specified\n"); + return irq; + } + + ret = devm_request_irq(dev, irq, sema4_irq_handler, 0, "SEMA4", NULL); + if (ret) { + dev_err(&pdev->dev, "failed to claim irq %u\n", irq); + return ret; + } + + // clear the gates table + for (i = 0; i < NUM_GATES; i++) + gates[i] = NULL; + + writel_relaxed(0, sema4_base + SEMA4_CP0INE); + + // debugfs + debugfs_dir = debugfs_create_dir(DEBUGFS_DIR, NULL); + + return 0; +} + +static const struct of_device_id vf610_sema4_dt_ids[] = { + { .compatible = "fsl,vf610-sema4" }, + { /* sentinel */ } +}; + +static struct platform_driver vf610_sema4_compat_driver = { + .driver = { + .name = "vf610-sema4", + .owner = THIS_MODULE, + .of_match_table = vf610_sema4_dt_ids, + }, + .probe = vf610_sema4_probe, +}; + +static int __init vf610_sema4_compat_init(void) +{ + return platform_driver_register(&vf610_sema4_compat_driver); +} +device_initcall(vf610_sema4_compat_init); + +MODULE_DESCRIPTION("Freescale SEMA4 driver"); +MODULE_LICENSE("GPL v2"); |