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