217 lines · c
1// SPDX-License-Identifier: GPL-2.02#include "fbtft.h"3#include "internal.h"4 5static int get_next_ulong(char **str_p, unsigned long *val, char *sep, int base)6{7 char *p_val;8 9 if (!str_p || !(*str_p))10 return -EINVAL;11 12 p_val = strsep(str_p, sep);13 14 if (!p_val)15 return -EINVAL;16 17 return kstrtoul(p_val, base, val);18}19 20int fbtft_gamma_parse_str(struct fbtft_par *par, u32 *curves,21 const char *str, int size)22{23 char *str_p, *curve_p = NULL;24 char *tmp;25 unsigned long val = 0;26 int ret = 0;27 int curve_counter, value_counter;28 int _count;29 30 if (!str || !curves)31 return -EINVAL;32 33 tmp = kmemdup(str, size + 1, GFP_KERNEL);34 if (!tmp)35 return -ENOMEM;36 37 /* replace optional separators */38 str_p = tmp;39 while (*str_p) {40 if (*str_p == ',')41 *str_p = ' ';42 if (*str_p == ';')43 *str_p = '\n';44 str_p++;45 }46 47 str_p = strim(tmp);48 49 curve_counter = 0;50 while (str_p) {51 if (curve_counter == par->gamma.num_curves) {52 dev_err(par->info->device, "Gamma: Too many curves\n");53 ret = -EINVAL;54 goto out;55 }56 curve_p = strsep(&str_p, "\n");57 value_counter = 0;58 while (curve_p) {59 if (value_counter == par->gamma.num_values) {60 dev_err(par->info->device,61 "Gamma: Too many values\n");62 ret = -EINVAL;63 goto out;64 }65 ret = get_next_ulong(&curve_p, &val, " ", 16);66 if (ret)67 goto out;68 69 _count = curve_counter * par->gamma.num_values +70 value_counter;71 curves[_count] = val;72 value_counter++;73 }74 if (value_counter != par->gamma.num_values) {75 dev_err(par->info->device, "Gamma: Too few values\n");76 ret = -EINVAL;77 goto out;78 }79 curve_counter++;80 }81 if (curve_counter != par->gamma.num_curves) {82 dev_err(par->info->device, "Gamma: Too few curves\n");83 ret = -EINVAL;84 goto out;85 }86 87out:88 kfree(tmp);89 return ret;90}91 92static ssize_t93sprintf_gamma(struct fbtft_par *par, u32 *curves, char *buf)94{95 ssize_t len = 0;96 unsigned int i, j;97 98 mutex_lock(&par->gamma.lock);99 for (i = 0; i < par->gamma.num_curves; i++) {100 for (j = 0; j < par->gamma.num_values; j++)101 len += scnprintf(&buf[len], PAGE_SIZE,102 "%04x ", curves[i * par->gamma.num_values + j]);103 buf[len - 1] = '\n';104 }105 mutex_unlock(&par->gamma.lock);106 107 return len;108}109 110static ssize_t store_gamma_curve(struct device *device,111 struct device_attribute *attr,112 const char *buf, size_t count)113{114 struct fb_info *fb_info = dev_get_drvdata(device);115 struct fbtft_par *par = fb_info->par;116 u32 tmp_curves[FBTFT_GAMMA_MAX_VALUES_TOTAL];117 int ret;118 119 ret = fbtft_gamma_parse_str(par, tmp_curves, buf, count);120 if (ret)121 return ret;122 123 ret = par->fbtftops.set_gamma(par, tmp_curves);124 if (ret)125 return ret;126 127 mutex_lock(&par->gamma.lock);128 memcpy(par->gamma.curves, tmp_curves,129 par->gamma.num_curves * par->gamma.num_values *130 sizeof(tmp_curves[0]));131 mutex_unlock(&par->gamma.lock);132 133 return count;134}135 136static ssize_t show_gamma_curve(struct device *device,137 struct device_attribute *attr, char *buf)138{139 struct fb_info *fb_info = dev_get_drvdata(device);140 struct fbtft_par *par = fb_info->par;141 142 return sprintf_gamma(par, par->gamma.curves, buf);143}144 145static struct device_attribute gamma_device_attrs[] = {146 __ATTR(gamma, 0660, show_gamma_curve, store_gamma_curve),147};148 149void fbtft_expand_debug_value(unsigned long *debug)150{151 switch (*debug & 0x7) {152 case 1:153 *debug |= DEBUG_LEVEL_1;154 break;155 case 2:156 *debug |= DEBUG_LEVEL_2;157 break;158 case 3:159 *debug |= DEBUG_LEVEL_3;160 break;161 case 4:162 *debug |= DEBUG_LEVEL_4;163 break;164 case 5:165 *debug |= DEBUG_LEVEL_5;166 break;167 case 6:168 *debug |= DEBUG_LEVEL_6;169 break;170 case 7:171 *debug = 0xFFFFFFFF;172 break;173 }174}175 176static ssize_t store_debug(struct device *device,177 struct device_attribute *attr,178 const char *buf, size_t count)179{180 struct fb_info *fb_info = dev_get_drvdata(device);181 struct fbtft_par *par = fb_info->par;182 int ret;183 184 ret = kstrtoul(buf, 10, &par->debug);185 if (ret)186 return ret;187 fbtft_expand_debug_value(&par->debug);188 189 return count;190}191 192static ssize_t show_debug(struct device *device,193 struct device_attribute *attr, char *buf)194{195 struct fb_info *fb_info = dev_get_drvdata(device);196 struct fbtft_par *par = fb_info->par;197 198 return sysfs_emit(buf, "%lu\n", par->debug);199}200 201static struct device_attribute debug_device_attr =202 __ATTR(debug, 0660, show_debug, store_debug);203 204void fbtft_sysfs_init(struct fbtft_par *par)205{206 device_create_file(par->info->dev, &debug_device_attr);207 if (par->gamma.curves && par->fbtftops.set_gamma)208 device_create_file(par->info->dev, &gamma_device_attrs[0]);209}210 211void fbtft_sysfs_exit(struct fbtft_par *par)212{213 device_remove_file(par->info->dev, &debug_device_attr);214 if (par->gamma.curves && par->fbtftops.set_gamma)215 device_remove_file(par->info->dev, &gamma_device_attrs[0]);216}217