Position measurement works

diff --git a/motorek.c b/motorek.c
index 6be72fcd92cb1bdce1aa779bd83f9a03045096bd..d0633e937a73c962128aac72c55ee3de35898236 100644 (file)
--- a/motorek.c
+++ b/motorek.c
@@ -7,10 +7,11 @@
 #include <linux/interrupt.h>
 
 struct motorek {
-       struct mpc52xx_gpt *pwmf, *pwmb;
+       struct mpc52xx_gpt *pwmf, *pwmb, *irca, *ircb;
        atomic_t pos;
        int irq;
        int action;
+       int last_irc_state;
 };
 
 static ssize_t show_position(struct device *dev,
@@ -55,7 +56,13 @@ DEVICE_ATTR(action,0644,show_action,store_action);
 #define MPC52xx_GPT_MODE_PWM     3
 #define MPC52xx_GPT_MODE_GPIO    4
 
-#define MPC52xx_GPT_PWM_OP       (1<<8)
+#define MPC52xx_GPT_MODE_GPIO_IN   (0<<4)
+#define MPC52xx_GPT_MODE_GPIO_OUT0 (2<<4)
+#define MPC52xx_GPT_MODE_GPIO_OUT1 (1<<4)
+
+#define MPC52xx_GPT_STATUS_PIN (1<<8)
+
+#define MPC52xx_GPT_PWM_OP       (1<<8) /* PWM polarity */
 
 /* Base clock 132 MHz
  * (/proc/device-tree/cpus/PowerPC,5200@0/bus-frequency) */
@@ -69,7 +76,7 @@ static void pwm_width(struct mpc52xx_gpt *gpt, u16 width)
 
 }
 
-static void __devinit pwm_init(struct mpc52xx_gpt *gpt)
+static void __devinit init_pwn(struct mpc52xx_gpt *gpt)
 {
        //while (((unsigned)gpt & 0xff) != 0x50) gpt++;
        out_be32(&gpt->count, (1<<16) | PWM_PERIOD);
@@ -82,6 +89,13 @@ static void __devinit pwm_init(struct mpc52xx_gpt *gpt)
        //printk("pwm: %p\n", gpt);
 }
 
+static void __devinit init_input(struct mpc52xx_gpt *gpt)
+{
+       out_be32(&gpt->mode,
+                MPC52xx_GPT_MODE_GPIO |
+                MPC52xx_GPT_MODE_GPIO_IN);
+}
+
 static void  pwm_done(struct mpc52xx_gpt *gpt)
 {
        out_be32(&gpt->mode, 0);
@@ -103,9 +117,10 @@ static void motorek_action(struct motorek *m, int action_permile)
 
 static int __devinit motorek_init(struct motorek *m)
 {
-       pwm_init(m->pwmf);
-       pwm_init(m->pwmb);
-       //motorek_action(m, +0);
+       init_pwn(m->pwmf);
+       init_pwn(m->pwmb);
+       init_input(m->irca);
+       init_input(m->ircb);
        return 0;
 }
 
@@ -115,26 +130,57 @@ static int motorek_done(struct motorek *m)
        pwm_done(m->pwmb);
        return 0;
 }
+#define TRANSITION(old, new) (((old) << 2) | new)
+static const int table[16] = {
+       [TRANSITION(0,1)] = +1,
+       [TRANSITION(1,3)] = +1,
+       [TRANSITION(3,2)] = +1,
+       [TRANSITION(2,0)] = +1,
+
+       [TRANSITION(1,0)] = -1,
+       [TRANSITION(3,1)] = -1,
+       [TRANSITION(2,3)] = -1,
+       [TRANSITION(0,2)] = -1,
+};
 
 static irqreturn_t motorek_irq(int irq, void *dev_id)
 {
        struct motorek *m = dev_id;
-       atomic_inc(&m->pos);
+       int irc_state =
+               ((in_be32(&m->irca->status) & MPC52xx_GPT_STATUS_PIN) ? 1 : 0) | \
+               ((in_be32(&m->ircb->status) & MPC52xx_GPT_STATUS_PIN) ? 2 : 0);
+       atomic_add(table[TRANSITION(m->last_irc_state, irc_state)], &m->pos);
+       m->last_irc_state = irc_state;
        return IRQ_HANDLED;
 }
 
-struct mpc52xx_gpt __iomem *iomap_gpt_by_phandle(const phandle *phandle)
+static struct of_device_id mpc5200_gpt_match[] = {
+       { .compatible = "mpc5200-gpt", },
+       { .compatible = "fsl,mpc5200-gpt", },
+       {},
+};
+
+struct mpc52xx_gpt __iomem *iomap_gpt_by_phandle_prop(struct device_node *dn, const char *prop)
 {
        struct device_node *np;
-       struct mpc52xx_gpt __iomem *gpt;
+       struct mpc52xx_gpt __iomem *gpt = NULL;
+       const phandle *phandle;
 
+       phandle = of_get_property(dn, prop, NULL);
        if (!phandle)
                return NULL;
 
        np = of_find_node_by_phandle(*phandle);
        if (!np)
                return NULL;
+
+       if (!of_match_node(mpc5200_gpt_match, np)) {
+               printk(KERN_ERR "property %s doesn't refer to GPT\n", prop);
+               goto out;
+       }
+       
        gpt = of_iomap(np, 0);
+out:
        of_node_put(np);
        return gpt;
 }
@@ -152,26 +198,29 @@ static int __devinit motorek_probe(struct of_device* dev,
 
        err = -ENODEV;
        
-       m->pwmf = iomap_gpt_by_phandle(of_get_property(dn, "pwmf", NULL));
+       m->pwmf = iomap_gpt_by_phandle_prop(dn, "pwmf");
        if (!m->pwmf)
                goto err_free;
-       
-       m->pwmb = iomap_gpt_by_phandle(of_get_property(dn, "pwmb", NULL));
+       m->pwmb = iomap_gpt_by_phandle_prop(dn, "pwmb");
        if (!m->pwmb)
-               goto err_unmapf;
+               goto err_unmap_pf;
+       m->irca = iomap_gpt_by_phandle_prop(dn, "irca");
+       if (!m->irca)
+               goto err_unmap_pb;
+       m->ircb = iomap_gpt_by_phandle_prop(dn, "ircb");
+       if (!m->ircb)
+               goto err_unmap_ia;
        
        m->irq = irq_of_parse_and_map(dn, 0);
        if (m->irq == NO_IRQ)
-               goto err_unmapb;
-
+               goto err_unmap_ib;
        err = request_irq(m->irq, motorek_irq, 0, "motorek", m);
        if (err)
-               goto err_unmapb;
+               goto err_unmap_ib;
 
        motorek_init(m);
 
        platform_set_drvdata(dev, m);
-
        err = device_create_file(&dev->dev,&dev_attr_position);
        if (err)
                goto err_irq;
@@ -185,9 +234,13 @@ static int __devinit motorek_probe(struct of_device* dev,
 
 err_irq:
        free_irq(m->irq, m);
-err_unmapb:
+err_unmap_ib:
+       iounmap(m->pwmb);
+err_unmap_ia:
+       iounmap(m->pwmf);
+err_unmap_pb:
        iounmap(m->pwmb);
-err_unmapf:
+err_unmap_pf:
        iounmap(m->pwmf);
 err_free:
        kfree(m);