Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  *
0004  * FIXME: Properly make this race free with refcounting etc...
0005  *
0006  * FIXME: LOCKING !!!
0007  */
0008 
0009 #include <linux/delay.h>
0010 #include <linux/kernel.h>
0011 #include <linux/spinlock.h>
0012 #include <linux/slab.h>
0013 #include <linux/module.h>
0014 #include <linux/mutex.h>
0015 #include <linux/of.h>
0016 
0017 #include <asm/pmac_pfunc.h>
0018 
0019 /* Debug */
0020 #define LOG_PARSE(fmt...)
0021 #define LOG_ERROR(fmt...)   printk(fmt)
0022 #define LOG_BLOB(t,b,c)
0023 
0024 #undef DEBUG
0025 #ifdef DEBUG
0026 #define DBG(fmt...)     printk(fmt)
0027 #else
0028 #define DBG(fmt...)
0029 #endif
0030 
0031 /* Command numbers */
0032 #define PMF_CMD_LIST            0
0033 #define PMF_CMD_WRITE_GPIO      1
0034 #define PMF_CMD_READ_GPIO       2
0035 #define PMF_CMD_WRITE_REG32     3
0036 #define PMF_CMD_READ_REG32      4
0037 #define PMF_CMD_WRITE_REG16     5
0038 #define PMF_CMD_READ_REG16      6
0039 #define PMF_CMD_WRITE_REG8      7
0040 #define PMF_CMD_READ_REG8       8
0041 #define PMF_CMD_DELAY           9
0042 #define PMF_CMD_WAIT_REG32      10
0043 #define PMF_CMD_WAIT_REG16      11
0044 #define PMF_CMD_WAIT_REG8       12
0045 #define PMF_CMD_READ_I2C        13
0046 #define PMF_CMD_WRITE_I2C       14
0047 #define PMF_CMD_RMW_I2C         15
0048 #define PMF_CMD_GEN_I2C         16
0049 #define PMF_CMD_SHIFT_BYTES_RIGHT   17
0050 #define PMF_CMD_SHIFT_BYTES_LEFT    18
0051 #define PMF_CMD_READ_CFG        19
0052 #define PMF_CMD_WRITE_CFG       20
0053 #define PMF_CMD_RMW_CFG         21
0054 #define PMF_CMD_READ_I2C_SUBADDR    22
0055 #define PMF_CMD_WRITE_I2C_SUBADDR   23
0056 #define PMF_CMD_SET_I2C_MODE        24
0057 #define PMF_CMD_RMW_I2C_SUBADDR     25
0058 #define PMF_CMD_READ_REG32_MASK_SHR_XOR 26
0059 #define PMF_CMD_READ_REG16_MASK_SHR_XOR 27
0060 #define PMF_CMD_READ_REG8_MASK_SHR_XOR  28
0061 #define PMF_CMD_WRITE_REG32_SHL_MASK    29
0062 #define PMF_CMD_WRITE_REG16_SHL_MASK    30
0063 #define PMF_CMD_WRITE_REG8_SHL_MASK 31
0064 #define PMF_CMD_MASK_AND_COMPARE    32
0065 #define PMF_CMD_COUNT           33
0066 
0067 /* This structure holds the state of the parser while walking through
0068  * a function definition
0069  */
0070 struct pmf_cmd {
0071     const void      *cmdptr;
0072     const void      *cmdend;
0073     struct pmf_function *func;
0074     void            *instdata;
0075     struct pmf_args     *args;
0076     int         error;
0077 };
0078 
0079 #if 0
0080 /* Debug output */
0081 static void print_blob(const char *title, const void *blob, int bytes)
0082 {
0083     printk("%s", title);
0084     while(bytes--) {
0085         printk("%02x ", *((u8 *)blob));
0086         blob += 1;
0087     }
0088     printk("\n");
0089 }
0090 #endif
0091 
0092 /*
0093  * Parser helpers
0094  */
0095 
0096 static u32 pmf_next32(struct pmf_cmd *cmd)
0097 {
0098     u32 value;
0099     if ((cmd->cmdend - cmd->cmdptr) < 4) {
0100         cmd->error = 1;
0101         return 0;
0102     }
0103     value = *((u32 *)cmd->cmdptr);
0104     cmd->cmdptr += 4;
0105     return value;
0106 }
0107 
0108 static const void* pmf_next_blob(struct pmf_cmd *cmd, int count)
0109 {
0110     const void *value;
0111     if ((cmd->cmdend - cmd->cmdptr) < count) {
0112         cmd->error = 1;
0113         return NULL;
0114     }
0115     value = cmd->cmdptr;
0116     cmd->cmdptr += count;
0117     return value;
0118 }
0119 
0120 /*
0121  * Individual command parsers
0122  */
0123 
0124 #define PMF_PARSE_CALL(name, cmd, handlers, p...) \
0125     do { \
0126         if (cmd->error) \
0127             return -ENXIO; \
0128         if (handlers == NULL) \
0129             return 0; \
0130         if (handlers->name)                   \
0131             return handlers->name(cmd->func, cmd->instdata, \
0132                           cmd->args, p);          \
0133         return -1; \
0134     } while(0) \
0135 
0136 
0137 static int pmf_parser_write_gpio(struct pmf_cmd *cmd, struct pmf_handlers *h)
0138 {
0139     u8 value = (u8)pmf_next32(cmd);
0140     u8 mask = (u8)pmf_next32(cmd);
0141 
0142     LOG_PARSE("pmf: write_gpio(value: %02x, mask: %02x)\n", value, mask);
0143 
0144     PMF_PARSE_CALL(write_gpio, cmd, h, value, mask);
0145 }
0146 
0147 static int pmf_parser_read_gpio(struct pmf_cmd *cmd, struct pmf_handlers *h)
0148 {
0149     u8 mask = (u8)pmf_next32(cmd);
0150     int rshift = (int)pmf_next32(cmd);
0151     u8 xor = (u8)pmf_next32(cmd);
0152 
0153     LOG_PARSE("pmf: read_gpio(mask: %02x, rshift: %d, xor: %02x)\n",
0154           mask, rshift, xor);
0155 
0156     PMF_PARSE_CALL(read_gpio, cmd, h, mask, rshift, xor);
0157 }
0158 
0159 static int pmf_parser_write_reg32(struct pmf_cmd *cmd, struct pmf_handlers *h)
0160 {
0161     u32 offset = pmf_next32(cmd);
0162     u32 value = pmf_next32(cmd);
0163     u32 mask = pmf_next32(cmd);
0164 
0165     LOG_PARSE("pmf: write_reg32(offset: %08x, value: %08x, mask: %08x)\n",
0166           offset, value, mask);
0167 
0168     PMF_PARSE_CALL(write_reg32, cmd, h, offset, value, mask);
0169 }
0170 
0171 static int pmf_parser_read_reg32(struct pmf_cmd *cmd, struct pmf_handlers *h)
0172 {
0173     u32 offset = pmf_next32(cmd);
0174 
0175     LOG_PARSE("pmf: read_reg32(offset: %08x)\n", offset);
0176 
0177     PMF_PARSE_CALL(read_reg32, cmd, h, offset);
0178 }
0179 
0180 
0181 static int pmf_parser_write_reg16(struct pmf_cmd *cmd, struct pmf_handlers *h)
0182 {
0183     u32 offset = pmf_next32(cmd);
0184     u16 value = (u16)pmf_next32(cmd);
0185     u16 mask = (u16)pmf_next32(cmd);
0186 
0187     LOG_PARSE("pmf: write_reg16(offset: %08x, value: %04x, mask: %04x)\n",
0188           offset, value, mask);
0189 
0190     PMF_PARSE_CALL(write_reg16, cmd, h, offset, value, mask);
0191 }
0192 
0193 static int pmf_parser_read_reg16(struct pmf_cmd *cmd, struct pmf_handlers *h)
0194 {
0195     u32 offset = pmf_next32(cmd);
0196 
0197     LOG_PARSE("pmf: read_reg16(offset: %08x)\n", offset);
0198 
0199     PMF_PARSE_CALL(read_reg16, cmd, h, offset);
0200 }
0201 
0202 
0203 static int pmf_parser_write_reg8(struct pmf_cmd *cmd, struct pmf_handlers *h)
0204 {
0205     u32 offset = pmf_next32(cmd);
0206     u8 value = (u16)pmf_next32(cmd);
0207     u8 mask = (u16)pmf_next32(cmd);
0208 
0209     LOG_PARSE("pmf: write_reg8(offset: %08x, value: %02x, mask: %02x)\n",
0210           offset, value, mask);
0211 
0212     PMF_PARSE_CALL(write_reg8, cmd, h, offset, value, mask);
0213 }
0214 
0215 static int pmf_parser_read_reg8(struct pmf_cmd *cmd, struct pmf_handlers *h)
0216 {
0217     u32 offset = pmf_next32(cmd);
0218 
0219     LOG_PARSE("pmf: read_reg8(offset: %08x)\n", offset);
0220 
0221     PMF_PARSE_CALL(read_reg8, cmd, h, offset);
0222 }
0223 
0224 static int pmf_parser_delay(struct pmf_cmd *cmd, struct pmf_handlers *h)
0225 {
0226     u32 duration = pmf_next32(cmd);
0227 
0228     LOG_PARSE("pmf: delay(duration: %d us)\n", duration);
0229 
0230     PMF_PARSE_CALL(delay, cmd, h, duration);
0231 }
0232 
0233 static int pmf_parser_wait_reg32(struct pmf_cmd *cmd, struct pmf_handlers *h)
0234 {
0235     u32 offset = pmf_next32(cmd);
0236     u32 value = pmf_next32(cmd);
0237     u32 mask = pmf_next32(cmd);
0238 
0239     LOG_PARSE("pmf: wait_reg32(offset: %08x, comp_value: %08x,mask: %08x)\n",
0240           offset, value, mask);
0241 
0242     PMF_PARSE_CALL(wait_reg32, cmd, h, offset, value, mask);
0243 }
0244 
0245 static int pmf_parser_wait_reg16(struct pmf_cmd *cmd, struct pmf_handlers *h)
0246 {
0247     u32 offset = pmf_next32(cmd);
0248     u16 value = (u16)pmf_next32(cmd);
0249     u16 mask = (u16)pmf_next32(cmd);
0250 
0251     LOG_PARSE("pmf: wait_reg16(offset: %08x, comp_value: %04x,mask: %04x)\n",
0252           offset, value, mask);
0253 
0254     PMF_PARSE_CALL(wait_reg16, cmd, h, offset, value, mask);
0255 }
0256 
0257 static int pmf_parser_wait_reg8(struct pmf_cmd *cmd, struct pmf_handlers *h)
0258 {
0259     u32 offset = pmf_next32(cmd);
0260     u8 value = (u8)pmf_next32(cmd);
0261     u8 mask = (u8)pmf_next32(cmd);
0262 
0263     LOG_PARSE("pmf: wait_reg8(offset: %08x, comp_value: %02x,mask: %02x)\n",
0264           offset, value, mask);
0265 
0266     PMF_PARSE_CALL(wait_reg8, cmd, h, offset, value, mask);
0267 }
0268 
0269 static int pmf_parser_read_i2c(struct pmf_cmd *cmd, struct pmf_handlers *h)
0270 {
0271     u32 bytes = pmf_next32(cmd);
0272 
0273     LOG_PARSE("pmf: read_i2c(bytes: %ud)\n", bytes);
0274 
0275     PMF_PARSE_CALL(read_i2c, cmd, h, bytes);
0276 }
0277 
0278 static int pmf_parser_write_i2c(struct pmf_cmd *cmd, struct pmf_handlers *h)
0279 {
0280     u32 bytes = pmf_next32(cmd);
0281     const void *blob = pmf_next_blob(cmd, bytes);
0282 
0283     LOG_PARSE("pmf: write_i2c(bytes: %ud) ...\n", bytes);
0284     LOG_BLOB("pmf:   data: \n", blob, bytes);
0285 
0286     PMF_PARSE_CALL(write_i2c, cmd, h, bytes, blob);
0287 }
0288 
0289 
0290 static int pmf_parser_rmw_i2c(struct pmf_cmd *cmd, struct pmf_handlers *h)
0291 {
0292     u32 maskbytes = pmf_next32(cmd);
0293     u32 valuesbytes = pmf_next32(cmd);
0294     u32 totalbytes = pmf_next32(cmd);
0295     const void *maskblob = pmf_next_blob(cmd, maskbytes);
0296     const void *valuesblob = pmf_next_blob(cmd, valuesbytes);
0297 
0298     LOG_PARSE("pmf: rmw_i2c(maskbytes: %ud, valuebytes: %ud, "
0299           "totalbytes: %d) ...\n",
0300           maskbytes, valuesbytes, totalbytes);
0301     LOG_BLOB("pmf:   mask data: \n", maskblob, maskbytes);
0302     LOG_BLOB("pmf:   values data: \n", valuesblob, valuesbytes);
0303 
0304     PMF_PARSE_CALL(rmw_i2c, cmd, h, maskbytes, valuesbytes, totalbytes,
0305                maskblob, valuesblob);
0306 }
0307 
0308 static int pmf_parser_read_cfg(struct pmf_cmd *cmd, struct pmf_handlers *h)
0309 {
0310     u32 offset = pmf_next32(cmd);
0311     u32 bytes = pmf_next32(cmd);
0312 
0313     LOG_PARSE("pmf: read_cfg(offset: %x, bytes: %ud)\n", offset, bytes);
0314 
0315     PMF_PARSE_CALL(read_cfg, cmd, h, offset, bytes);
0316 }
0317 
0318 
0319 static int pmf_parser_write_cfg(struct pmf_cmd *cmd, struct pmf_handlers *h)
0320 {
0321     u32 offset = pmf_next32(cmd);
0322     u32 bytes = pmf_next32(cmd);
0323     const void *blob = pmf_next_blob(cmd, bytes);
0324 
0325     LOG_PARSE("pmf: write_cfg(offset: %x, bytes: %ud)\n", offset, bytes);
0326     LOG_BLOB("pmf:   data: \n", blob, bytes);
0327 
0328     PMF_PARSE_CALL(write_cfg, cmd, h, offset, bytes, blob);
0329 }
0330 
0331 static int pmf_parser_rmw_cfg(struct pmf_cmd *cmd, struct pmf_handlers *h)
0332 {
0333     u32 offset = pmf_next32(cmd);
0334     u32 maskbytes = pmf_next32(cmd);
0335     u32 valuesbytes = pmf_next32(cmd);
0336     u32 totalbytes = pmf_next32(cmd);
0337     const void *maskblob = pmf_next_blob(cmd, maskbytes);
0338     const void *valuesblob = pmf_next_blob(cmd, valuesbytes);
0339 
0340     LOG_PARSE("pmf: rmw_cfg(maskbytes: %ud, valuebytes: %ud,"
0341           " totalbytes: %d) ...\n",
0342           maskbytes, valuesbytes, totalbytes);
0343     LOG_BLOB("pmf:   mask data: \n", maskblob, maskbytes);
0344     LOG_BLOB("pmf:   values data: \n", valuesblob, valuesbytes);
0345 
0346     PMF_PARSE_CALL(rmw_cfg, cmd, h, offset, maskbytes, valuesbytes,
0347                totalbytes, maskblob, valuesblob);
0348 }
0349 
0350 
0351 static int pmf_parser_read_i2c_sub(struct pmf_cmd *cmd, struct pmf_handlers *h)
0352 {
0353     u8 subaddr = (u8)pmf_next32(cmd);
0354     u32 bytes = pmf_next32(cmd);
0355 
0356     LOG_PARSE("pmf: read_i2c_sub(subaddr: %x, bytes: %ud)\n",
0357           subaddr, bytes);
0358 
0359     PMF_PARSE_CALL(read_i2c_sub, cmd, h, subaddr, bytes);
0360 }
0361 
0362 static int pmf_parser_write_i2c_sub(struct pmf_cmd *cmd, struct pmf_handlers *h)
0363 {
0364     u8 subaddr = (u8)pmf_next32(cmd);
0365     u32 bytes = pmf_next32(cmd);
0366     const void *blob = pmf_next_blob(cmd, bytes);
0367 
0368     LOG_PARSE("pmf: write_i2c_sub(subaddr: %x, bytes: %ud) ...\n",
0369           subaddr, bytes);
0370     LOG_BLOB("pmf:   data: \n", blob, bytes);
0371 
0372     PMF_PARSE_CALL(write_i2c_sub, cmd, h, subaddr, bytes, blob);
0373 }
0374 
0375 static int pmf_parser_set_i2c_mode(struct pmf_cmd *cmd, struct pmf_handlers *h)
0376 {
0377     u32 mode = pmf_next32(cmd);
0378 
0379     LOG_PARSE("pmf: set_i2c_mode(mode: %d)\n", mode);
0380 
0381     PMF_PARSE_CALL(set_i2c_mode, cmd, h, mode);
0382 }
0383 
0384 
0385 static int pmf_parser_rmw_i2c_sub(struct pmf_cmd *cmd, struct pmf_handlers *h)
0386 {
0387     u8 subaddr = (u8)pmf_next32(cmd);
0388     u32 maskbytes = pmf_next32(cmd);
0389     u32 valuesbytes = pmf_next32(cmd);
0390     u32 totalbytes = pmf_next32(cmd);
0391     const void *maskblob = pmf_next_blob(cmd, maskbytes);
0392     const void *valuesblob = pmf_next_blob(cmd, valuesbytes);
0393 
0394     LOG_PARSE("pmf: rmw_i2c_sub(subaddr: %x, maskbytes: %ud, valuebytes: %ud"
0395           ", totalbytes: %d) ...\n",
0396           subaddr, maskbytes, valuesbytes, totalbytes);
0397     LOG_BLOB("pmf:   mask data: \n", maskblob, maskbytes);
0398     LOG_BLOB("pmf:   values data: \n", valuesblob, valuesbytes);
0399 
0400     PMF_PARSE_CALL(rmw_i2c_sub, cmd, h, subaddr, maskbytes, valuesbytes,
0401                totalbytes, maskblob, valuesblob);
0402 }
0403 
0404 static int pmf_parser_read_reg32_msrx(struct pmf_cmd *cmd,
0405                       struct pmf_handlers *h)
0406 {
0407     u32 offset = pmf_next32(cmd);
0408     u32 mask = pmf_next32(cmd);
0409     u32 shift = pmf_next32(cmd);
0410     u32 xor = pmf_next32(cmd);
0411 
0412     LOG_PARSE("pmf: read_reg32_msrx(offset: %x, mask: %x, shift: %x,"
0413           " xor: %x\n", offset, mask, shift, xor);
0414 
0415     PMF_PARSE_CALL(read_reg32_msrx, cmd, h, offset, mask, shift, xor);
0416 }
0417 
0418 static int pmf_parser_read_reg16_msrx(struct pmf_cmd *cmd,
0419                       struct pmf_handlers *h)
0420 {
0421     u32 offset = pmf_next32(cmd);
0422     u32 mask = pmf_next32(cmd);
0423     u32 shift = pmf_next32(cmd);
0424     u32 xor = pmf_next32(cmd);
0425 
0426     LOG_PARSE("pmf: read_reg16_msrx(offset: %x, mask: %x, shift: %x,"
0427           " xor: %x\n", offset, mask, shift, xor);
0428 
0429     PMF_PARSE_CALL(read_reg16_msrx, cmd, h, offset, mask, shift, xor);
0430 }
0431 static int pmf_parser_read_reg8_msrx(struct pmf_cmd *cmd,
0432                      struct pmf_handlers *h)
0433 {
0434     u32 offset = pmf_next32(cmd);
0435     u32 mask = pmf_next32(cmd);
0436     u32 shift = pmf_next32(cmd);
0437     u32 xor = pmf_next32(cmd);
0438 
0439     LOG_PARSE("pmf: read_reg8_msrx(offset: %x, mask: %x, shift: %x,"
0440           " xor: %x\n", offset, mask, shift, xor);
0441 
0442     PMF_PARSE_CALL(read_reg8_msrx, cmd, h, offset, mask, shift, xor);
0443 }
0444 
0445 static int pmf_parser_write_reg32_slm(struct pmf_cmd *cmd,
0446                       struct pmf_handlers *h)
0447 {
0448     u32 offset = pmf_next32(cmd);
0449     u32 shift = pmf_next32(cmd);
0450     u32 mask = pmf_next32(cmd);
0451 
0452     LOG_PARSE("pmf: write_reg32_slm(offset: %x, shift: %x, mask: %x\n",
0453           offset, shift, mask);
0454 
0455     PMF_PARSE_CALL(write_reg32_slm, cmd, h, offset, shift, mask);
0456 }
0457 
0458 static int pmf_parser_write_reg16_slm(struct pmf_cmd *cmd,
0459                       struct pmf_handlers *h)
0460 {
0461     u32 offset = pmf_next32(cmd);
0462     u32 shift = pmf_next32(cmd);
0463     u32 mask = pmf_next32(cmd);
0464 
0465     LOG_PARSE("pmf: write_reg16_slm(offset: %x, shift: %x, mask: %x\n",
0466           offset, shift, mask);
0467 
0468     PMF_PARSE_CALL(write_reg16_slm, cmd, h, offset, shift, mask);
0469 }
0470 
0471 static int pmf_parser_write_reg8_slm(struct pmf_cmd *cmd,
0472                      struct pmf_handlers *h)
0473 {
0474     u32 offset = pmf_next32(cmd);
0475     u32 shift = pmf_next32(cmd);
0476     u32 mask = pmf_next32(cmd);
0477 
0478     LOG_PARSE("pmf: write_reg8_slm(offset: %x, shift: %x, mask: %x\n",
0479           offset, shift, mask);
0480 
0481     PMF_PARSE_CALL(write_reg8_slm, cmd, h, offset, shift, mask);
0482 }
0483 
0484 static int pmf_parser_mask_and_compare(struct pmf_cmd *cmd,
0485                        struct pmf_handlers *h)
0486 {
0487     u32 bytes = pmf_next32(cmd);
0488     const void *maskblob = pmf_next_blob(cmd, bytes);
0489     const void *valuesblob = pmf_next_blob(cmd, bytes);
0490 
0491     LOG_PARSE("pmf: mask_and_compare(length: %ud ...\n", bytes);
0492     LOG_BLOB("pmf:   mask data: \n", maskblob, bytes);
0493     LOG_BLOB("pmf:   values data: \n", valuesblob, bytes);
0494 
0495     PMF_PARSE_CALL(mask_and_compare, cmd, h,
0496                bytes, maskblob, valuesblob);
0497 }
0498 
0499 
0500 typedef int (*pmf_cmd_parser_t)(struct pmf_cmd *cmd, struct pmf_handlers *h);
0501 
0502 static pmf_cmd_parser_t pmf_parsers[PMF_CMD_COUNT] =
0503 {
0504     NULL,
0505     pmf_parser_write_gpio,
0506     pmf_parser_read_gpio,
0507     pmf_parser_write_reg32,
0508     pmf_parser_read_reg32,
0509     pmf_parser_write_reg16,
0510     pmf_parser_read_reg16,
0511     pmf_parser_write_reg8,
0512     pmf_parser_read_reg8,
0513     pmf_parser_delay,
0514     pmf_parser_wait_reg32,
0515     pmf_parser_wait_reg16,
0516     pmf_parser_wait_reg8,
0517     pmf_parser_read_i2c,
0518     pmf_parser_write_i2c,
0519     pmf_parser_rmw_i2c,
0520     NULL, /* Bogus command */
0521     NULL, /* Shift bytes right: NYI */
0522     NULL, /* Shift bytes left: NYI */
0523     pmf_parser_read_cfg,
0524     pmf_parser_write_cfg,
0525     pmf_parser_rmw_cfg,
0526     pmf_parser_read_i2c_sub,
0527     pmf_parser_write_i2c_sub,
0528     pmf_parser_set_i2c_mode,
0529     pmf_parser_rmw_i2c_sub,
0530     pmf_parser_read_reg32_msrx,
0531     pmf_parser_read_reg16_msrx,
0532     pmf_parser_read_reg8_msrx,
0533     pmf_parser_write_reg32_slm,
0534     pmf_parser_write_reg16_slm,
0535     pmf_parser_write_reg8_slm,
0536     pmf_parser_mask_and_compare,
0537 };
0538 
0539 struct pmf_device {
0540     struct list_head    link;
0541     struct device_node  *node;
0542     struct pmf_handlers *handlers;
0543     struct list_head    functions;
0544     struct kref     ref;
0545 };
0546 
0547 static LIST_HEAD(pmf_devices);
0548 static DEFINE_SPINLOCK(pmf_lock);
0549 static DEFINE_MUTEX(pmf_irq_mutex);
0550 
0551 static void pmf_release_device(struct kref *kref)
0552 {
0553     struct pmf_device *dev = container_of(kref, struct pmf_device, ref);
0554     kfree(dev);
0555 }
0556 
0557 static inline void pmf_put_device(struct pmf_device *dev)
0558 {
0559     kref_put(&dev->ref, pmf_release_device);
0560 }
0561 
0562 static inline struct pmf_device *pmf_get_device(struct pmf_device *dev)
0563 {
0564     kref_get(&dev->ref);
0565     return dev;
0566 }
0567 
0568 static inline struct pmf_device *pmf_find_device(struct device_node *np)
0569 {
0570     struct pmf_device *dev;
0571 
0572     list_for_each_entry(dev, &pmf_devices, link) {
0573         if (dev->node == np)
0574             return pmf_get_device(dev);
0575     }
0576     return NULL;
0577 }
0578 
0579 static int pmf_parse_one(struct pmf_function *func,
0580              struct pmf_handlers *handlers,
0581              void *instdata, struct pmf_args *args)
0582 {
0583     struct pmf_cmd cmd;
0584     u32 ccode;
0585     int count, rc;
0586 
0587     cmd.cmdptr      = func->data;
0588     cmd.cmdend      = func->data + func->length;
0589     cmd.func            = func;
0590     cmd.instdata        = instdata;
0591     cmd.args        = args;
0592     cmd.error       = 0;
0593 
0594     LOG_PARSE("pmf: func %s, %d bytes, %s...\n",
0595           func->name, func->length,
0596           handlers ? "executing" : "parsing");
0597 
0598     /* One subcommand to parse for now */
0599     count = 1;
0600 
0601     while(count-- && cmd.cmdptr < cmd.cmdend) {
0602         /* Get opcode */
0603         ccode = pmf_next32(&cmd);
0604         /* Check if we are hitting a command list, fetch new count */
0605         if (ccode == 0) {
0606             count = pmf_next32(&cmd) - 1;
0607             ccode = pmf_next32(&cmd);
0608         }
0609         if (cmd.error) {
0610             LOG_ERROR("pmf: parse error, not enough data\n");
0611             return -ENXIO;
0612         }
0613         if (ccode >= PMF_CMD_COUNT) {
0614             LOG_ERROR("pmf: command code %d unknown !\n", ccode);
0615             return -ENXIO;
0616         }
0617         if (pmf_parsers[ccode] == NULL) {
0618             LOG_ERROR("pmf: no parser for command %d !\n", ccode);
0619             return -ENXIO;
0620         }
0621         rc = pmf_parsers[ccode](&cmd, handlers);
0622         if (rc != 0) {
0623             LOG_ERROR("pmf: parser for command %d returned"
0624                   " error %d\n", ccode, rc);
0625             return rc;
0626         }
0627     }
0628 
0629     /* We are doing an initial parse pass, we need to adjust the size */
0630     if (handlers == NULL)
0631         func->length = cmd.cmdptr - func->data;
0632 
0633     return 0;
0634 }
0635 
0636 static int pmf_add_function_prop(struct pmf_device *dev, void *driverdata,
0637                  const char *name, u32 *data,
0638                  unsigned int length)
0639 {
0640     int count = 0;
0641     struct pmf_function *func = NULL;
0642 
0643     DBG("pmf: Adding functions for platform-do-%s\n", name);
0644 
0645     while (length >= 12) {
0646         /* Allocate a structure */
0647         func = kzalloc(sizeof(*func), GFP_KERNEL);
0648         if (func == NULL)
0649             goto bail;
0650         kref_init(&func->ref);
0651         INIT_LIST_HEAD(&func->irq_clients);
0652         func->node = dev->node;
0653         func->driver_data = driverdata;
0654         func->name = name;
0655         func->phandle = data[0];
0656         func->flags = data[1];
0657         data += 2;
0658         length -= 8;
0659         func->data = data;
0660         func->length = length;
0661         func->dev = dev;
0662         DBG("pmf: idx %d: flags=%08x, phandle=%08x "
0663             " %d bytes remaining, parsing...\n",
0664             count+1, func->flags, func->phandle, length);
0665         if (pmf_parse_one(func, NULL, NULL, NULL)) {
0666             kfree(func);
0667             goto bail;
0668         }
0669         length -= func->length;
0670         data = (u32 *)(((u8 *)data) + func->length);
0671         list_add(&func->link, &dev->functions);
0672         pmf_get_device(dev);
0673         count++;
0674     }
0675  bail:
0676     DBG("pmf: Added %d functions\n", count);
0677 
0678     return count;
0679 }
0680 
0681 static int pmf_add_functions(struct pmf_device *dev, void *driverdata)
0682 {
0683     struct property *pp;
0684 #define PP_PREFIX "platform-do-"
0685     const int plen = strlen(PP_PREFIX);
0686     int count = 0;
0687 
0688     for_each_property_of_node(dev->node, pp) {
0689         const char *name;
0690         if (strncmp(pp->name, PP_PREFIX, plen) != 0)
0691             continue;
0692         name = pp->name + plen;
0693         if (strlen(name) && pp->length >= 12)
0694             count += pmf_add_function_prop(dev, driverdata, name,
0695                                pp->value, pp->length);
0696     }
0697     return count;
0698 }
0699 
0700 
0701 int pmf_register_driver(struct device_node *np,
0702             struct pmf_handlers *handlers,
0703             void *driverdata)
0704 {
0705     struct pmf_device *dev;
0706     unsigned long flags;
0707     int rc = 0;
0708 
0709     if (handlers == NULL)
0710         return -EINVAL;
0711 
0712     DBG("pmf: registering driver for node %pOF\n", np);
0713 
0714     spin_lock_irqsave(&pmf_lock, flags);
0715     dev = pmf_find_device(np);
0716     spin_unlock_irqrestore(&pmf_lock, flags);
0717     if (dev != NULL) {
0718         DBG("pmf: already there !\n");
0719         pmf_put_device(dev);
0720         return -EBUSY;
0721     }
0722 
0723     dev = kzalloc(sizeof(*dev), GFP_KERNEL);
0724     if (dev == NULL) {
0725         DBG("pmf: no memory !\n");
0726         return -ENOMEM;
0727     }
0728     kref_init(&dev->ref);
0729     dev->node = of_node_get(np);
0730     dev->handlers = handlers;
0731     INIT_LIST_HEAD(&dev->functions);
0732 
0733     rc = pmf_add_functions(dev, driverdata);
0734     if (rc == 0) {
0735         DBG("pmf: no functions, disposing.. \n");
0736         of_node_put(np);
0737         kfree(dev);
0738         return -ENODEV;
0739     }
0740 
0741     spin_lock_irqsave(&pmf_lock, flags);
0742     list_add(&dev->link, &pmf_devices);
0743     spin_unlock_irqrestore(&pmf_lock, flags);
0744 
0745     return 0;
0746 }
0747 EXPORT_SYMBOL_GPL(pmf_register_driver);
0748 
0749 struct pmf_function *pmf_get_function(struct pmf_function *func)
0750 {
0751     if (!try_module_get(func->dev->handlers->owner))
0752         return NULL;
0753     kref_get(&func->ref);
0754     return func;
0755 }
0756 EXPORT_SYMBOL_GPL(pmf_get_function);
0757 
0758 static void pmf_release_function(struct kref *kref)
0759 {
0760     struct pmf_function *func =
0761         container_of(kref, struct pmf_function, ref);
0762     pmf_put_device(func->dev);
0763     kfree(func);
0764 }
0765 
0766 static inline void __pmf_put_function(struct pmf_function *func)
0767 {
0768     kref_put(&func->ref, pmf_release_function);
0769 }
0770 
0771 void pmf_put_function(struct pmf_function *func)
0772 {
0773     if (func == NULL)
0774         return;
0775     module_put(func->dev->handlers->owner);
0776     __pmf_put_function(func);
0777 }
0778 EXPORT_SYMBOL_GPL(pmf_put_function);
0779 
0780 void pmf_unregister_driver(struct device_node *np)
0781 {
0782     struct pmf_device *dev;
0783     unsigned long flags;
0784 
0785     DBG("pmf: unregistering driver for node %pOF\n", np);
0786 
0787     spin_lock_irqsave(&pmf_lock, flags);
0788     dev = pmf_find_device(np);
0789     if (dev == NULL) {
0790         DBG("pmf: not such driver !\n");
0791         spin_unlock_irqrestore(&pmf_lock, flags);
0792         return;
0793     }
0794     list_del(&dev->link);
0795 
0796     while(!list_empty(&dev->functions)) {
0797         struct pmf_function *func =
0798             list_entry(dev->functions.next, typeof(*func), link);
0799         list_del(&func->link);
0800         __pmf_put_function(func);
0801     }
0802 
0803     pmf_put_device(dev);
0804     spin_unlock_irqrestore(&pmf_lock, flags);
0805 }
0806 EXPORT_SYMBOL_GPL(pmf_unregister_driver);
0807 
0808 static struct pmf_function *__pmf_find_function(struct device_node *target,
0809                      const char *name, u32 flags)
0810 {
0811     struct device_node *actor = of_node_get(target);
0812     struct pmf_device *dev;
0813     struct pmf_function *func, *result = NULL;
0814     char fname[64];
0815     const u32 *prop;
0816     u32 ph;
0817 
0818     /*
0819      * Look for a "platform-*" function reference. If we can't find
0820      * one, then we fallback to a direct call attempt
0821      */
0822     snprintf(fname, 63, "platform-%s", name);
0823     prop = of_get_property(target, fname, NULL);
0824     if (prop == NULL)
0825         goto find_it;
0826     ph = *prop;
0827     if (ph == 0)
0828         goto find_it;
0829 
0830     /*
0831      * Ok, now try to find the actor. If we can't find it, we fail,
0832      * there is no point in falling back there
0833      */
0834     of_node_put(actor);
0835     actor = of_find_node_by_phandle(ph);
0836     if (actor == NULL)
0837         return NULL;
0838  find_it:
0839     dev = pmf_find_device(actor);
0840     if (dev == NULL) {
0841         result = NULL;
0842         goto out;
0843     }
0844 
0845     list_for_each_entry(func, &dev->functions, link) {
0846         if (name && strcmp(name, func->name))
0847             continue;
0848         if (func->phandle && target->phandle != func->phandle)
0849             continue;
0850         if ((func->flags & flags) == 0)
0851             continue;
0852         result = func;
0853         break;
0854     }
0855     pmf_put_device(dev);
0856 out:
0857     of_node_put(actor);
0858     return result;
0859 }
0860 
0861 
0862 int pmf_register_irq_client(struct device_node *target,
0863                 const char *name,
0864                 struct pmf_irq_client *client)
0865 {
0866     struct pmf_function *func;
0867     unsigned long flags;
0868 
0869     spin_lock_irqsave(&pmf_lock, flags);
0870     func = __pmf_find_function(target, name, PMF_FLAGS_INT_GEN);
0871     if (func)
0872         func = pmf_get_function(func);
0873     spin_unlock_irqrestore(&pmf_lock, flags);
0874     if (func == NULL)
0875         return -ENODEV;
0876 
0877     /* guard against manipulations of list */
0878     mutex_lock(&pmf_irq_mutex);
0879     if (list_empty(&func->irq_clients))
0880         func->dev->handlers->irq_enable(func);
0881 
0882     /* guard against pmf_do_irq while changing list */
0883     spin_lock_irqsave(&pmf_lock, flags);
0884     list_add(&client->link, &func->irq_clients);
0885     spin_unlock_irqrestore(&pmf_lock, flags);
0886 
0887     client->func = func;
0888     mutex_unlock(&pmf_irq_mutex);
0889 
0890     return 0;
0891 }
0892 EXPORT_SYMBOL_GPL(pmf_register_irq_client);
0893 
0894 void pmf_unregister_irq_client(struct pmf_irq_client *client)
0895 {
0896     struct pmf_function *func = client->func;
0897     unsigned long flags;
0898 
0899     BUG_ON(func == NULL);
0900 
0901     /* guard against manipulations of list */
0902     mutex_lock(&pmf_irq_mutex);
0903     client->func = NULL;
0904 
0905     /* guard against pmf_do_irq while changing list */
0906     spin_lock_irqsave(&pmf_lock, flags);
0907     list_del(&client->link);
0908     spin_unlock_irqrestore(&pmf_lock, flags);
0909 
0910     if (list_empty(&func->irq_clients))
0911         func->dev->handlers->irq_disable(func);
0912     mutex_unlock(&pmf_irq_mutex);
0913     pmf_put_function(func);
0914 }
0915 EXPORT_SYMBOL_GPL(pmf_unregister_irq_client);
0916 
0917 
0918 void pmf_do_irq(struct pmf_function *func)
0919 {
0920     unsigned long flags;
0921     struct pmf_irq_client *client;
0922 
0923     /* For now, using a spinlock over the whole function. Can be made
0924      * to drop the lock using 2 lists if necessary
0925      */
0926     spin_lock_irqsave(&pmf_lock, flags);
0927     list_for_each_entry(client, &func->irq_clients, link) {
0928         if (!try_module_get(client->owner))
0929             continue;
0930         client->handler(client->data);
0931         module_put(client->owner);
0932     }
0933     spin_unlock_irqrestore(&pmf_lock, flags);
0934 }
0935 EXPORT_SYMBOL_GPL(pmf_do_irq);
0936 
0937 
0938 int pmf_call_one(struct pmf_function *func, struct pmf_args *args)
0939 {
0940     struct pmf_device *dev = func->dev;
0941     void *instdata = NULL;
0942     int rc = 0;
0943 
0944     DBG(" ** pmf_call_one(%pOF/%s) **\n", dev->node, func->name);
0945 
0946     if (dev->handlers->begin)
0947         instdata = dev->handlers->begin(func, args);
0948     rc = pmf_parse_one(func, dev->handlers, instdata, args);
0949     if (dev->handlers->end)
0950         dev->handlers->end(func, instdata);
0951 
0952     return rc;
0953 }
0954 EXPORT_SYMBOL_GPL(pmf_call_one);
0955 
0956 int pmf_do_functions(struct device_node *np, const char *name,
0957              u32 phandle, u32 fflags, struct pmf_args *args)
0958 {
0959     struct pmf_device *dev;
0960     struct pmf_function *func, *tmp;
0961     unsigned long flags;
0962     int rc = -ENODEV;
0963 
0964     spin_lock_irqsave(&pmf_lock, flags);
0965 
0966     dev = pmf_find_device(np);
0967     if (dev == NULL) {
0968         spin_unlock_irqrestore(&pmf_lock, flags);
0969         return -ENODEV;
0970     }
0971     list_for_each_entry_safe(func, tmp, &dev->functions, link) {
0972         if (name && strcmp(name, func->name))
0973             continue;
0974         if (phandle && func->phandle && phandle != func->phandle)
0975             continue;
0976         if ((func->flags & fflags) == 0)
0977             continue;
0978         if (pmf_get_function(func) == NULL)
0979             continue;
0980         spin_unlock_irqrestore(&pmf_lock, flags);
0981         rc = pmf_call_one(func, args);
0982         pmf_put_function(func);
0983         spin_lock_irqsave(&pmf_lock, flags);
0984     }
0985     pmf_put_device(dev);
0986     spin_unlock_irqrestore(&pmf_lock, flags);
0987 
0988     return rc;
0989 }
0990 EXPORT_SYMBOL_GPL(pmf_do_functions);
0991 
0992 
0993 struct pmf_function *pmf_find_function(struct device_node *target,
0994                        const char *name)
0995 {
0996     struct pmf_function *func;
0997     unsigned long flags;
0998 
0999     spin_lock_irqsave(&pmf_lock, flags);
1000     func = __pmf_find_function(target, name, PMF_FLAGS_ON_DEMAND);
1001     if (func)
1002         func = pmf_get_function(func);
1003     spin_unlock_irqrestore(&pmf_lock, flags);
1004     return func;
1005 }
1006 EXPORT_SYMBOL_GPL(pmf_find_function);
1007 
1008 int pmf_call_function(struct device_node *target, const char *name,
1009               struct pmf_args *args)
1010 {
1011     struct pmf_function *func = pmf_find_function(target, name);
1012     int rc;
1013 
1014     if (func == NULL)
1015         return -ENODEV;
1016 
1017     rc = pmf_call_one(func, args);
1018     pmf_put_function(func);
1019     return rc;
1020 }
1021 EXPORT_SYMBOL_GPL(pmf_call_function);
1022
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!