--- zzzz-none-000/linux-4.4.60/fs/debugfs/file.c 2017-04-08 07:53:53.000000000 +0000 +++ jet-2400-727/linux-4.4.60/fs/debugfs/file.c 2021-03-17 14:36:41.000000000 +0000 @@ -22,6 +22,7 @@ #include #include #include +#include static ssize_t default_read_file(struct file *file, char __user *buf, size_t count, loff_t *ppos) @@ -724,19 +725,139 @@ for (i = 0; i < nregs; i++, regs++) { if (prefix) seq_printf(s, "%s", prefix); - seq_printf(s, "%s = 0x%08x\n", regs->name, - readl(base + regs->offset)); + seq_printf(s, "%#x: %s = 0x%08x\n",(unsigned int)(base + regs->offset), + regs->name, readl(base + regs->offset)); if (seq_has_overflowed(s)) break; } } EXPORT_SYMBOL_GPL(debugfs_print_regs32); +#if defined(CONFIG_AVM_ENHANCED) +static void debugfs_print_regs32_with_read_fn(struct seq_file *s, + struct debugfs_regset32 *regset) +{ + const struct debugfs_reg32 *reg = regset->regs; + int i; + + for (i = 0; i < regset->nregs; i++, reg++) { + uint32_t addr = (uint32_t)regset->base + reg->offset; + seq_printf(s, "%#x: %s = 0x%08x\n", addr ,reg->name, + regset->reg_read_fn(regset->rw_context, addr)); + } + return; +} + +/* + * this write function allows writing to our regset like this: + * echo 'example_reg=0xdeadbeaf' > /sys/kernel/debug/our_regset + */ +static ssize_t debugfs_write_regset32(struct file *file, + const char *buffer, + size_t count, + loff_t *offset __maybe_unused) { + + unsigned char *debugfs_buffer; + unsigned char *val_buffer = NULL; + ssize_t fs_res = -EINVAL; + size_t debugfs_buffer_size; + struct debugfs_regset32 *regset; + unsigned int i; + + regset =((struct seq_file *) file->private_data)->private; + pr_debug("[%s] enter\n", __func__); + + /* + * get buffer size: + * we need one extra byte in our buffer: the terminating 0-byte + * count variable contains user data len, this data is not 0-terminated + */ + if (count >= SD_MAX_INPUT_BUF ) { + debugfs_buffer_size = SD_MAX_INPUT_BUF; + } + else { + debugfs_buffer_size = count + 1; + } + + /* alloc buffer */ + debugfs_buffer = vmalloc(debugfs_buffer_size); + if (!debugfs_buffer){ + fs_res = -ENOMEM; + goto exit; + } + + /* write data to the buffer */ + if ( copy_from_user(debugfs_buffer, buffer, debugfs_buffer_size - 1) ) { + fs_res = -EFAULT; + goto free_exit; + } + + + debugfs_buffer[debugfs_buffer_size - 1] = 0; + + /* looking for '=' */ + for (i = 0; i < debugfs_buffer_size - 1; i ++){ + if (debugfs_buffer[i] == '='){ + debugfs_buffer[i] = 0; + val_buffer = &debugfs_buffer[i+1]; + } + } + if ( !val_buffer ){ + goto free_exit; + } + + for (i = 0; i < regset->nregs; i++){ + if ( strncmp(regset->regs[i].name, debugfs_buffer, (val_buffer - debugfs_buffer)) == 0) { + const struct debugfs_reg32 *reg = ®set->regs[i]; + unsigned int new_val; + pr_debug("lookup successful: %s in %d\n", debugfs_buffer, i); + if ( kstrtouint( val_buffer, 0, &new_val) == 0){ + pr_debug("new_val = %d\n", new_val); + fs_res = debugfs_buffer_size - 1; + if ( regset->reg_write_fn ){ + int reg_write_res; + pr_debug("using write fn = %pF\n", regset->reg_write_fn); + reg_write_res = regset->reg_write_fn(regset->rw_context, + (uint32_t)regset->base + reg->offset, + new_val); + if (reg_write_res < 0 ) + fs_res = -EIO; + } else { + pr_debug("no write fn, doing direct mem access at %#x + %#x\n", + (unsigned int)regset->base , (unsigned int)reg->offset ); + writel(new_val, regset->base + reg->offset); + } + } + goto free_exit; + } + } + +free_exit: + vfree(debugfs_buffer); +exit: + return fs_res; +} + + +#endif + + static int debugfs_show_regset32(struct seq_file *s, void *data) { struct debugfs_regset32 *regset = s->private; +#if defined(CONFIG_AVM_ENHANCED) + + if (regset->reg_read_fn) + debugfs_print_regs32_with_read_fn(s, regset); + else + debugfs_print_regs32(s, regset->regs, + regset->nregs, regset->base, ""); + +#else debugfs_print_regs32(s, regset->regs, regset->nregs, regset->base, ""); + +#endif /* defined(CONFIG_AVM_ENHANCED) */ return 0; } @@ -750,6 +871,9 @@ .read = seq_read, .llseek = seq_lseek, .release = single_release, +#if defined(CONFIG_AVM_ENHANCED) + .write = debugfs_write_regset32, +#endif }; /** @@ -787,6 +911,161 @@ #endif /* CONFIG_HAS_IOMEM */ +#if defined(CONFIG_AVM_ENHANCED) + +LIST_HEAD(simple_debugfs_file_list); + +struct simple_debugfs_file_internal { + struct list_head list; + struct file_operations fops; + struct dentry *dentry; + struct dentry *parent; + int (*kernel_input)(char *, void *); + void (*kernel_output) (struct seq_file*, void *); + void *priv_data; + size_t expected_output_buffer_size; +}; + +static int simple_debugfs_file_show(struct seq_file *m, + void *offset __maybe_unused) { + struct simple_debugfs_file_internal *sdf = m->private; + if (sdf->kernel_output){ + pr_debug( "offset: %p\n", offset); + sdf->kernel_output(m, sdf->priv_data); + } + return 0; +} + +static int simple_debugfs_file_open(struct inode *inode, struct file *file) { + unsigned int res; + struct simple_debugfs_file_internal *sdf = inode->i_private; + pr_debug("call open with simple_debugfs_file_entry %p: priv=%p\n", + sdf, sdf->priv_data); + + if (sdf->expected_output_buffer_size){ + res = single_open_size(file, simple_debugfs_file_show, + sdf, sdf->expected_output_buffer_size); + } else { + res = single_open(file, simple_debugfs_file_show, sdf); + } + return res; +} + +static ssize_t simple_debugfs_file_write(struct file *file, + const char *buffer, + size_t count, + loff_t *offset __maybe_unused) { + + unsigned char *debugfs_buffer; + ssize_t res; + size_t debugfs_buffer_size; + struct simple_debugfs_file_internal *sdf; + + sdf =((struct seq_file *) file->private_data)->private; + + if ( sdf->kernel_input == NULL ) + return -EFAULT; + + /* + * get buffer size: + * we need one extra byte in our buffer: the terminating 0-byte + * count variable contains user data len, this data is not 0-terminated + */ + if (count >= SD_MAX_INPUT_BUF ) { + debugfs_buffer_size = SD_MAX_INPUT_BUF; + } + else { + debugfs_buffer_size = count + 1; + } + + /* alloc buffer */ + debugfs_buffer = vmalloc(debugfs_buffer_size); + if (!debugfs_buffer){ + return -ENOMEM; + } + + /* write data to the buffer */ + if ( copy_from_user(debugfs_buffer, buffer, debugfs_buffer_size - 1) ) { + vfree(debugfs_buffer); + return -EFAULT; + } + + debugfs_buffer[debugfs_buffer_size - 1] = 0; + res = sdf->kernel_input( debugfs_buffer, sdf->priv_data ); + if (res >= 0 ) + res = debugfs_buffer_size - 1; + + vfree(debugfs_buffer); + return res; +} + +int add_simple_debugfs_file( const char* fname, + struct dentry *parent, + int (*kernel_input)(char *, void *), + void (*kernel_output)(struct seq_file *, void *), + void *priv_data) +{ + + struct simple_debugfs_file_internal *simple_debugfs_file_entry = NULL; + + /* alloc private data structure */ + simple_debugfs_file_entry = + kzalloc( sizeof(*simple_debugfs_file_entry), GFP_KERNEL ); + if (!simple_debugfs_file_entry) { + return -ENOMEM; + } + + /* setup fops */ + simple_debugfs_file_entry->fops.open = simple_debugfs_file_open; + simple_debugfs_file_entry->fops.read = seq_read; + simple_debugfs_file_entry->fops.llseek = seq_lseek; + simple_debugfs_file_entry->fops.release = single_release; + if ( kernel_input ){ + simple_debugfs_file_entry->fops.write = + simple_debugfs_file_write; + } + + /* setup more */ + simple_debugfs_file_entry->parent = parent; + simple_debugfs_file_entry->priv_data = priv_data; + simple_debugfs_file_entry->kernel_input = kernel_input; + simple_debugfs_file_entry->kernel_output = kernel_output; + + /* create debugfs file */ + simple_debugfs_file_entry->dentry = debugfs_create_file( + fname, 0444, parent, + simple_debugfs_file_entry, + &simple_debugfs_file_entry->fops + ); + if (! simple_debugfs_file_entry->dentry ) { + kfree( simple_debugfs_file_entry ); + pr_debug("debugfs_crate_data failed \n"); + return -EFAULT; + } + + list_add(&simple_debugfs_file_entry->list, &simple_debugfs_file_list); + pr_debug("register simple_debugfs_file_entry %p\n", + simple_debugfs_file_entry); + return 0; +} + +void remove_simple_debugfs_file( struct dentry *dentry ){ + struct simple_debugfs_file_internal *sdf; + + list_for_each_entry(sdf, &simple_debugfs_file_list, list) { + if ( sdf->dentry == dentry ) { + list_del( &sdf->list ); + debugfs_remove( dentry ); + pr_debug("remove entry \n" ); + return ; + } + } +} + +EXPORT_SYMBOL(add_simple_debugfs_file); +EXPORT_SYMBOL(remove_simple_debugfs_file); + +#endif /* CONFIG_AVM_ENHANCED */ struct debugfs_devm_entry { int (*read)(struct seq_file *seq, void *data); struct device *dev;