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 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
87 out:
88 kfree(tmp);
89 return ret;
90 }
91
92 static ssize_t
sprintf_gamma(struct fbtft_par * par,u32 * curves,char * buf)93 sprintf_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
store_gamma_curve(struct device * device,struct device_attribute * attr,const char * buf,size_t count)110 static 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
show_gamma_curve(struct device * device,struct device_attribute * attr,char * buf)136 static 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
145 static struct device_attribute gamma_device_attrs[] = {
146 __ATTR(gamma, 0660, show_gamma_curve, store_gamma_curve),
147 };
148
fbtft_expand_debug_value(unsigned long * debug)149 void 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
store_debug(struct device * device,struct device_attribute * attr,const char * buf,size_t count)176 static 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
show_debug(struct device * device,struct device_attribute * attr,char * buf)192 static 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
201 static struct device_attribute debug_device_attr =
202 __ATTR(debug, 0660, show_debug, store_debug);
203
fbtft_sysfs_init(struct fbtft_par * par)204 void 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
fbtft_sysfs_exit(struct fbtft_par * par)211 void 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