From: Michal Sojka <sojkam1@fel.cvut.cz>
Date: Fri, 16 Jan 2009 08:13:31 +0000 (+0100)
Subject: Position measurement works
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/shark/motorek-5200.git/commitdiff_plain/3fb646e97916179a1617faa701640322a45f23a5

Position measurement works
---

diff --git a/motorek.c b/motorek.c
index 6be72fc..d0633e9 100644
--- 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);