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