1 /* SPDX-License-Identifier: GPL-2.0 */
2 /*
3  * Traceprobe fetch helper inlines
4  */
5 
6 static nokprobe_inline void
7 fetch_store_raw(unsigned long val, struct fetch_insn *code, void *buf)
8 {
9 	switch (code->size) {
10 	case 1:
11 		*(u8 *)buf = (u8)val;
12 		break;
13 	case 2:
14 		*(u16 *)buf = (u16)val;
15 		break;
16 	case 4:
17 		*(u32 *)buf = (u32)val;
18 		break;
19 	case 8:
20 		//TBD: 32bit signed
21 		*(u64 *)buf = (u64)val;
22 		break;
23 	default:
24 		*(unsigned long *)buf = val;
25 	}
26 }
27 
28 static nokprobe_inline void
29 fetch_apply_bitfield(struct fetch_insn *code, void *buf)
30 {
31 	switch (code->basesize) {
32 	case 1:
33 		*(u8 *)buf <<= code->lshift;
34 		*(u8 *)buf >>= code->rshift;
35 		break;
36 	case 2:
37 		*(u16 *)buf <<= code->lshift;
38 		*(u16 *)buf >>= code->rshift;
39 		break;
40 	case 4:
41 		*(u32 *)buf <<= code->lshift;
42 		*(u32 *)buf >>= code->rshift;
43 		break;
44 	case 8:
45 		*(u64 *)buf <<= code->lshift;
46 		*(u64 *)buf >>= code->rshift;
47 		break;
48 	}
49 }
50 
51 /*
52  * These functions must be defined for each callsite.
53  * Return consumed dynamic data size (>= 0), or error (< 0).
54  * If dest is NULL, don't store result and return required dynamic data size.
55  */
56 static int
57 process_fetch_insn(struct fetch_insn *code, struct pt_regs *regs,
58 		   void *dest, void *base);
59 static nokprobe_inline int fetch_store_strlen(unsigned long addr);
60 static nokprobe_inline int
61 fetch_store_string(unsigned long addr, void *dest, void *base);
62 static nokprobe_inline int fetch_store_strlen_user(unsigned long addr);
63 static nokprobe_inline int
64 fetch_store_string_user(unsigned long addr, void *dest, void *base);
65 static nokprobe_inline int
66 probe_mem_read(void *dest, void *src, size_t size);
67 static nokprobe_inline int
68 probe_mem_read_user(void *dest, void *src, size_t size);
69 
70 /* From the 2nd stage, routine is same */
71 static nokprobe_inline int
72 process_fetch_insn_bottom(struct fetch_insn *code, unsigned long val,
73 			   void *dest, void *base)
74 {
75 	struct fetch_insn *s3 = NULL;
76 	int total = 0, ret = 0, i = 0;
77 	u32 loc = 0;
78 	unsigned long lval = val;
79 
80 stage2:
81 	/* 2nd stage: dereference memory if needed */
82 	do {
83 		if (code->op == FETCH_OP_DEREF) {
84 			lval = val;
85 			ret = probe_mem_read(&val, (void *)val + code->offset,
86 					     sizeof(val));
87 		} else if (code->op == FETCH_OP_UDEREF) {
88 			lval = val;
89 			ret = probe_mem_read_user(&val,
90 				 (void *)val + code->offset, sizeof(val));
91 		} else
92 			break;
93 		if (ret)
94 			return ret;
95 		code++;
96 	} while (1);
97 
98 	s3 = code;
99 stage3:
100 	/* 3rd stage: store value to buffer */
101 	if (unlikely(!dest)) {
102 		if (code->op == FETCH_OP_ST_STRING) {
103 			ret = fetch_store_strlen(val + code->offset);
104 			code++;
105 			goto array;
106 		} else if (code->op == FETCH_OP_ST_USTRING) {
107 			ret += fetch_store_strlen_user(val + code->offset);
108 			code++;
109 			goto array;
110 		} else
111 			return -EILSEQ;
112 	}
113 
114 	switch (code->op) {
115 	case FETCH_OP_ST_RAW:
116 		fetch_store_raw(val, code, dest);
117 		break;
118 	case FETCH_OP_ST_MEM:
119 		probe_mem_read(dest, (void *)val + code->offset, code->size);
120 		break;
121 	case FETCH_OP_ST_UMEM:
122 		probe_mem_read_user(dest, (void *)val + code->offset, code->size);
123 		break;
124 	case FETCH_OP_ST_STRING:
125 		loc = *(u32 *)dest;
126 		ret = fetch_store_string(val + code->offset, dest, base);
127 		break;
128 	case FETCH_OP_ST_USTRING:
129 		loc = *(u32 *)dest;
130 		ret = fetch_store_string_user(val + code->offset, dest, base);
131 		break;
132 	default:
133 		return -EILSEQ;
134 	}
135 	code++;
136 
137 	/* 4th stage: modify stored value if needed */
138 	if (code->op == FETCH_OP_MOD_BF) {
139 		fetch_apply_bitfield(code, dest);
140 		code++;
141 	}
142 
143 array:
144 	/* the last stage: Loop on array */
145 	if (code->op == FETCH_OP_LP_ARRAY) {
146 		total += ret;
147 		if (++i < code->param) {
148 			code = s3;
149 			if (s3->op != FETCH_OP_ST_STRING &&
150 			    s3->op != FETCH_OP_ST_USTRING) {
151 				dest += s3->size;
152 				val += s3->size;
153 				goto stage3;
154 			}
155 			code--;
156 			val = lval + sizeof(char *);
157 			if (dest) {
158 				dest += sizeof(u32);
159 				*(u32 *)dest = update_data_loc(loc, ret);
160 			}
161 			goto stage2;
162 		}
163 		code++;
164 		ret = total;
165 	}
166 
167 	return code->op == FETCH_OP_END ? ret : -EILSEQ;
168 }
169 
170 /* Sum up total data length for dynamic arrays (strings) */
171 static nokprobe_inline int
172 __get_data_size(struct trace_probe *tp, struct pt_regs *regs)
173 {
174 	struct probe_arg *arg;
175 	int i, len, ret = 0;
176 
177 	for (i = 0; i < tp->nr_args; i++) {
178 		arg = tp->args + i;
179 		if (unlikely(arg->dynamic)) {
180 			len = process_fetch_insn(arg->code, regs, NULL, NULL);
181 			if (len > 0)
182 				ret += len;
183 		}
184 	}
185 
186 	return ret;
187 }
188 
189 /* Store the value of each argument */
190 static nokprobe_inline void
191 store_trace_args(void *data, struct trace_probe *tp, struct pt_regs *regs,
192 		 int header_size, int maxlen)
193 {
194 	struct probe_arg *arg;
195 	void *base = data - header_size;
196 	void *dyndata = data + tp->size;
197 	u32 *dl;	/* Data location */
198 	int ret, i;
199 
200 	for (i = 0; i < tp->nr_args; i++) {
201 		arg = tp->args + i;
202 		dl = data + arg->offset;
203 		/* Point the dynamic data area if needed */
204 		if (unlikely(arg->dynamic))
205 			*dl = make_data_loc(maxlen, dyndata - base);
206 		ret = process_fetch_insn(arg->code, regs, dl, base);
207 		if (unlikely(ret < 0 && arg->dynamic)) {
208 			*dl = make_data_loc(0, dyndata - base);
209 		} else {
210 			dyndata += ret;
211 			maxlen -= ret;
212 		}
213 	}
214 }
215 
216 static inline int
217 print_probe_args(struct trace_seq *s, struct probe_arg *args, int nr_args,
218 		 u8 *data, void *field)
219 {
220 	void *p;
221 	int i, j;
222 
223 	for (i = 0; i < nr_args; i++) {
224 		struct probe_arg *a = args + i;
225 
226 		trace_seq_printf(s, " %s=", a->name);
227 		if (likely(!a->count)) {
228 			if (!a->type->print(s, data + a->offset, field))
229 				return -ENOMEM;
230 			continue;
231 		}
232 		trace_seq_putc(s, '{');
233 		p = data + a->offset;
234 		for (j = 0; j < a->count; j++) {
235 			if (!a->type->print(s, p, field))
236 				return -ENOMEM;
237 			trace_seq_putc(s, j == a->count - 1 ? '}' : ',');
238 			p += a->type->size;
239 		}
240 	}
241 	return 0;
242 }
243