motorek.c

   1 #include <linux/module.h>
   2 #include <linux/platform_device.h>
   3 #include <linux/mod_devicetable.h>
   4 #include <linux/of.h>
   5 #include <linux/of_platform.h>
   6 #include <asm-powerpc/mpc52xx.h>
   7 #include <linux/interrupt.h>
   8
   9 struct motorek {
  10         struct mpc52xx_gpt *pwmf, *pwmb;
  11         atomic_t pos;
  12         int irq;
  13         int action;
  14 };
  15
  16 static ssize_t show_position(struct device *dev,
  17                             struct device_attribute *attr, char *buf)
  18 {
  19         struct platform_device  *pdev = to_platform_device(dev);
  20         struct motorek          *m = platform_get_drvdata(pdev);
  21
  22         int len = snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&m->pos));
  23         return (len >= PAGE_SIZE) ? (PAGE_SIZE - 1) : len;
  24 }
  25
  26 static ssize_t show_action(struct device *dev,
  27                            struct device_attribute *attr, char *buf)
  28 {
  29         struct platform_device  *pdev = to_platform_device(dev);
  30         struct motorek          *m = platform_get_drvdata(pdev);
  31
  32         int len = snprintf(buf, PAGE_SIZE, "%d\n", m->action);
  33         return (len >= PAGE_SIZE) ? (PAGE_SIZE - 1) : len;
  34 }
  35
  36 static void motorek_action(struct motorek *m, int action_permile);
  37 static ssize_t store_action(struct device *dev, struct device_attribute *attr,
  38                             const char *buf, size_t count)
  39 {
  40         struct platform_device  *pdev = to_platform_device(dev);
  41         struct motorek          *m = platform_get_drvdata(pdev);
  42
  43         int a;
  44         sscanf(buf, "%d", &a);
  45         motorek_action(m, a);
  46         return strnlen(buf, PAGE_SIZE);
  47 }
  48
  49 DEVICE_ATTR(position,0444,show_position,NULL);
  50 DEVICE_ATTR(action,0644,show_action,store_action);
  51
  52 #define MPC52xx_GPT_MODE_DISABLED 0
  53 #define MPC52xx_GPT_MODE_INCAPT   1
  54 #define MPC52xx_GPT_MODE_OUTCMP   2
  55 #define MPC52xx_GPT_MODE_PWM      3
  56 #define MPC52xx_GPT_MODE_GPIO     4
  57
  58 #define MPC52xx_GPT_PWM_OP        (1<<8)
  59
  60 /* Base clock 132 MHz
  61  * (/proc/device-tree/cpus/PowerPC,5200@0/bus-frequency) */
  62 #define PWM_PERIOD 6600         /* = 132 000 kHz / 20 kHz  */
  63
  64
  65 static void pwm_width(struct mpc52xx_gpt *gpt, u16 width)
  66 {
  67         out_be32(&gpt->pwm, (width<<16) | MPC52xx_GPT_PWM_OP);
  68         //printk("pwm: %p=0x%x\n", gpt, width);
  69
  70 }
  71
  72 static void __devinit pwm_init(struct mpc52xx_gpt *gpt)
  73 {
  74         //while (((unsigned)gpt & 0xff) != 0x50) gpt++;
  75         out_be32(&gpt->count, (1<<16) | PWM_PERIOD);
  76         pwm_width(gpt, 0);
  77         out_be32(&gpt->mode, MPC52xx_GPT_MODE_PWM);
  78
  79         //out_be32(&gpt->pwm, (500<<16) | MPC52xx_GPT_PWM_OP); /* REMOVE ME */
  80
  81         //out_be32(&gpt->mode, MPC52xx_GPT_MODE_GPIO | (2<<4));
  82         //printk("pwm: %p\n", gpt);
  83 }
  84
  85 static void  pwm_done(struct mpc52xx_gpt *gpt)
  86 {
  87         out_be32(&gpt->mode, 0);
  88         out_be32(&gpt->count, 0);
  89         pwm_width(gpt, 0);
  90 }
  91
  92 static void motorek_action(struct motorek *m, int action_permile)
  93 {
  94         m->action = action_permile;
  95         if (action_permile >= 0) {
  96                 pwm_width(m->pwmb, 0);
  97                 pwm_width(m->pwmf, +action_permile*PWM_PERIOD/1000);
  98         } else {
  99                 pwm_width(m->pwmf, 0);
 100                 pwm_width(m->pwmb, -action_permile*PWM_PERIOD/1000);
 101         }
 102 }
 103
 104 static int __devinit motorek_init(struct motorek *m)
 105 {
 106         pwm_init(m->pwmf);
 107         pwm_init(m->pwmb);
 108         //motorek_action(m, +0);
 109         return 0;
 110 }
 111
 112 static int motorek_done(struct motorek *m)
 113 {
 114         pwm_done(m->pwmf);
 115         pwm_done(m->pwmb);
 116         return 0;
 117 }
 118
 119 static irqreturn_t motorek_irq(int irq, void *dev_id)
 120 {
 121         struct motorek *m = dev_id;
 122         atomic_inc(&m->pos);
 123         return IRQ_HANDLED;
 124 }
 125
 126 struct mpc52xx_gpt __iomem *iomap_gpt_by_phandle(const phandle *phandle)
 127 {
 128         struct device_node *np;
 129         struct mpc52xx_gpt __iomem *gpt;
 130
 131         if (!phandle)
 132                 return NULL;
 133
 134         np = of_find_node_by_phandle(*phandle);
 135         if (!np)
 136                 return NULL;
 137         gpt = of_iomap(np, 0);
 138         of_node_put(np);
 139         return gpt;
 140 }
 141
 142 static int __devinit motorek_probe(struct of_device* dev,
 143                                    const struct of_device_id *match)
 144 {
 145         struct device_node *dn = dev->node;
 146         struct motorek *m;
 147         int err;
 148
 149         m = kzalloc(sizeof(*m), GFP_KERNEL);
 150         if (!m)
 151                 return -ENOMEM;
 152
 153         err = -ENODEV;
 154
 155         m->pwmf = iomap_gpt_by_phandle(of_get_property(dn, "pwmf", NULL));
 156         if (!m->pwmf)
 157                 goto err_free;
 158
 159         m->pwmb = iomap_gpt_by_phandle(of_get_property(dn, "pwmb", NULL));
 160         if (!m->pwmb)
 161                 goto err_unmapf;
 162
 163         m->irq = irq_of_parse_and_map(dn, 0);
 164         if (m->irq == NO_IRQ)
 165                 goto err_unmapb;
 166
 167         err = request_irq(m->irq, motorek_irq, 0, "motorek", m);
 168         if (err)
 169                 goto err_unmapb;
 170
 171         motorek_init(m);
 172
 173         platform_set_drvdata(dev, m);
 174
 175         err = device_create_file(&dev->dev,&dev_attr_position);
 176         if (err)
 177                 goto err_irq;
 178         err = device_create_file(&dev->dev,&dev_attr_action);
 179         if (err)
 180                 goto err_irq;
 181
 182         printk(KERN_NOTICE "Motorek initialized\n");
 183
 184         return 0;
 185
 186 err_irq:
 187         free_irq(m->irq, m);
 188 err_unmapb:
 189         iounmap(m->pwmb);
 190 err_unmapf:
 191         iounmap(m->pwmf);
 192 err_free:
 193         kfree(m);
 194         return err;
 195 }
 196
 197 static int __devexit motorek_remove(struct of_device* dev)
 198 {
 199         struct motorek *m;
 200
 201         printk(KERN_NOTICE "Removing motorek\n");
 202         m = platform_get_drvdata(dev);
 203         free_irq(m->irq, m);
 204         motorek_done(m);
 205         kfree(m);
 206         return 0;
 207 }
 208
 209 static struct of_device_id motorek_match[] = {
 210         { .type = "motorek", },
 211         {},
 212 };
 213 static struct of_platform_driver motorek_driver = {
 214         .owner          = THIS_MODULE,
 215         .name           = "motorek",
 216         .match_table    = motorek_match,
 217         .probe          = motorek_probe,
 218         .remove         = __devexit_p(motorek_remove),
 219 };
 220
 221
 222 static int __init motorek_init_module(void)
 223 {
 224         int ret;
 225         struct of_device *dev;
 226         struct device_node *dn;
 227
 228         for_each_node_by_type(dn, "motorek") {
 229                 if (!of_find_device_by_node(dn)) {
 230                         dev = of_platform_device_create(dn, NULL, NULL);
 231                         if (!dev)
 232                                 return -ENOMEM;
 233                 }
 234         };
 235
 236         ret = of_register_platform_driver(&motorek_driver);
 237         return ret;
 238 }
 239
 240 static void __exit motorek_exit_module(void)
 241 {
 242         struct of_device *dev;
 243         struct device_node *dn;
 244
 245         for_each_node_by_type(dn, "motorek") {
 246                 while ((dev = of_find_device_by_node(dn))) {
 247                         of_device_unregister(dev);
 248                 }
 249         }
 250         of_unregister_platform_driver(&motorek_driver);
 251 }
 252
 253 module_init(motorek_init_module);
 254 module_exit(motorek_exit_module);
 255
 256
 257 MODULE_LICENSE("GPL v2");
 258 MODULE_VERSION("0.1");
 259 MODULE_AUTHOR("Michal Sojka <sojkam1@fel.cvut.cz>");