xref: /openbmc/libmctp/core.c (revision 6896d41e)
1 /* SPDX-License-Identifier: Apache-2.0 OR GPL-2.0-or-later */
2 
3 #include <assert.h>
4 #include <stdarg.h>
5 #include <stddef.h>
6 #include <stdint.h>
7 #include <stdio.h>
8 #include <stdlib.h>
9 #include <string.h>
10 
11 #undef pr_fmt
12 #define pr_fmt(fmt) "core: " fmt
13 
14 #include "libmctp.h"
15 #include "libmctp-alloc.h"
16 #include "libmctp-log.h"
17 
18 /* Internal data structures */
19 
20 struct mctp_bus {
21 	mctp_eid_t		eid;
22 	struct mctp_binding	*binding;
23 	bool			tx_enabled;
24 
25 	struct mctp_pktbuf	*tx_queue_head;
26 	struct mctp_pktbuf	*tx_queue_tail;
27 
28 	/* todo: routing */
29 };
30 
31 struct mctp_msg_ctx {
32 	uint8_t		src;
33 	uint8_t		dest;
34 	uint8_t		tag;
35 	uint8_t		last_seq;
36 	void		*buf;
37 	size_t		buf_size;
38 	size_t		buf_alloc_size;
39 };
40 
41 struct mctp {
42 	int			n_busses;
43 	struct mctp_bus		*busses;
44 
45 	/* Message RX callback */
46 	mctp_rx_fn		message_rx;
47 	void			*message_rx_data;
48 
49 	/* Message reassembly.
50 	 * @todo: flexible context count
51 	 */
52 	struct mctp_msg_ctx	msg_ctxs[16];
53 
54 	enum {
55 		ROUTE_ENDPOINT,
56 		ROUTE_BRIDGE,
57 	}			route_policy;
58 };
59 
60 #ifndef BUILD_ASSERT
61 #define BUILD_ASSERT(x) \
62 	do { (void)sizeof(char[0-(!(x))]); } while (0)
63 #endif
64 
65 #ifndef ARRAY_SIZE
66 #define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
67 #endif
68 
69 static int mctp_message_tx_on_bus(struct mctp *mctp, struct mctp_bus *bus,
70 		mctp_eid_t src, mctp_eid_t dest, void *msg, size_t msg_len);
71 
72 struct mctp_pktbuf *mctp_pktbuf_alloc(struct mctp_binding *binding, size_t len)
73 {
74 	struct mctp_pktbuf *buf;
75 	size_t size;
76 
77 	size = binding->pkt_size + binding->pkt_pad;
78 
79 	/* todo: pools */
80 	buf = __mctp_alloc(sizeof(*buf) + size);
81 
82 	buf->size = size;
83 	buf->start = binding->pkt_pad;
84 	buf->end = buf->start + len;
85 	buf->mctp_hdr_off = buf->start;
86 	buf->next = NULL;
87 
88 	return buf;
89 }
90 
91 void mctp_pktbuf_free(struct mctp_pktbuf *pkt)
92 {
93 	__mctp_free(pkt);
94 }
95 
96 struct mctp_hdr *mctp_pktbuf_hdr(struct mctp_pktbuf *pkt)
97 {
98 	return (void *)pkt->data + pkt->mctp_hdr_off;
99 }
100 
101 void *mctp_pktbuf_data(struct mctp_pktbuf *pkt)
102 {
103 	return (void *)pkt->data + pkt->mctp_hdr_off + sizeof(struct mctp_hdr);
104 }
105 
106 uint8_t mctp_pktbuf_size(struct mctp_pktbuf *pkt)
107 {
108 	return pkt->end - pkt->start;
109 }
110 
111 void *mctp_pktbuf_alloc_start(struct mctp_pktbuf *pkt, size_t size)
112 {
113 	assert(size <= pkt->start);
114 	pkt->start -= size;
115 	return pkt->data + pkt->start;
116 }
117 
118 void *mctp_pktbuf_alloc_end(struct mctp_pktbuf *pkt, size_t size)
119 {
120 	void *buf;
121 
122 	assert(size < (pkt->size - pkt->end));
123 	buf = pkt->data + pkt->end;
124 	pkt->end += size;
125 	return buf;
126 }
127 
128 int mctp_pktbuf_push(struct mctp_pktbuf *pkt, void *data, size_t len)
129 {
130 	void *p;
131 
132 	if (pkt->end + len > pkt->size)
133 		return -1;
134 
135 	p = pkt->data + pkt->end;
136 
137 	pkt->end += len;
138 	memcpy(p, data, len);
139 
140 	return 0;
141 }
142 
143 /* Message reassembly */
144 static struct mctp_msg_ctx *mctp_msg_ctx_lookup(struct mctp *mctp,
145 		uint8_t src, uint8_t dest, uint8_t tag)
146 {
147 	unsigned int i;
148 
149 	/* @todo: better lookup, if we add support for more outstanding
150 	 * message contexts */
151 	for (i = 0; i < ARRAY_SIZE(mctp->msg_ctxs); i++) {
152 		struct mctp_msg_ctx *ctx = &mctp->msg_ctxs[i];
153 		if (ctx->src == src && ctx->dest == dest && ctx->tag == tag)
154 			return ctx;
155 	}
156 
157 	return NULL;
158 }
159 
160 static struct mctp_msg_ctx *mctp_msg_ctx_create(struct mctp *mctp,
161 		uint8_t src, uint8_t dest, uint8_t tag)
162 {
163 	struct mctp_msg_ctx *ctx = NULL;
164 	unsigned int i;
165 
166 	for (i = 0; i < ARRAY_SIZE(mctp->msg_ctxs); i++) {
167 		struct mctp_msg_ctx *tmp = &mctp->msg_ctxs[i];
168 		if (!tmp->src) {
169 			ctx = tmp;
170 			break;
171 		}
172 	}
173 
174 	if (!ctx)
175 		return NULL;
176 
177 	ctx->src = src;
178 	ctx->dest = dest;
179 	ctx->tag = tag;
180 	ctx->buf_size = 0;
181 
182 	return ctx;
183 }
184 
185 static void mctp_msg_ctx_drop(struct mctp_msg_ctx *ctx)
186 {
187 	ctx->src = 0;
188 }
189 
190 static void mctp_msg_ctx_reset(struct mctp_msg_ctx *ctx)
191 {
192 	ctx->buf_size = 0;
193 }
194 
195 static int mctp_msg_ctx_add_pkt(struct mctp_msg_ctx *ctx,
196 		struct mctp_pktbuf *pkt)
197 {
198 	size_t len;
199 
200 	len = mctp_pktbuf_size(pkt) - sizeof(struct mctp_hdr);
201 
202 	if (ctx->buf_size + len > ctx->buf_alloc_size) {
203 		size_t new_alloc_size;
204 		void *lbuf;
205 
206 		/* @todo: finer-grained allocation, size limits */
207 		if (!ctx->buf_alloc_size) {
208 			new_alloc_size = 4096;
209 		} else {
210 			new_alloc_size = ctx->buf_alloc_size * 2;
211 		}
212 
213 		lbuf = __mctp_realloc(ctx->buf, new_alloc_size);
214 		if (lbuf) {
215 			ctx->buf = lbuf;
216 			ctx->buf_alloc_size = new_alloc_size;
217 		} else {
218 			__mctp_free(ctx->buf);
219 			return -1;
220 		}
221 	}
222 
223 	memcpy(ctx->buf + ctx->buf_size, mctp_pktbuf_data(pkt), len);
224 	ctx->buf_size += len;
225 
226 	return 0;
227 }
228 
229 /* Core API functions */
230 struct mctp *mctp_init(void)
231 {
232 	struct mctp *mctp;
233 
234 	mctp = __mctp_alloc(sizeof(*mctp));
235 	memset(mctp, 0, sizeof(*mctp));
236 
237 	return mctp;
238 }
239 
240 void mctp_destroy(struct mctp *mctp)
241 {
242 	int i;
243 
244 	/* Cleanup message assembly contexts */
245 	for (i = 0; i < ARRAY_SIZE(mctp->msg_ctxs); i++) {
246 		struct mctp_msg_ctx *tmp = &mctp->msg_ctxs[i];
247 		if (tmp->buf)
248 			__mctp_free(tmp->buf);
249 	}
250 
251 	__mctp_free(mctp->busses);
252 	__mctp_free(mctp);
253 }
254 
255 int mctp_set_rx_all(struct mctp *mctp, mctp_rx_fn fn, void *data)
256 {
257 	mctp->message_rx = fn;
258 	mctp->message_rx_data = data;
259 	return 0;
260 }
261 
262 static struct mctp_bus *find_bus_for_eid(struct mctp *mctp,
263 		mctp_eid_t dest __attribute__((unused)))
264 {
265 	/* for now, just use the first bus. For full routing support,
266 	 * we will need a table of neighbours */
267 	return &mctp->busses[0];
268 }
269 
270 int mctp_register_bus(struct mctp *mctp,
271 		struct mctp_binding *binding,
272 		mctp_eid_t eid)
273 {
274 	/* todo: multiple busses */
275 	assert(mctp->n_busses == 0);
276 	mctp->n_busses = 1;
277 	mctp->busses = __mctp_alloc(sizeof(struct mctp_bus));
278 	memset(mctp->busses, 0, sizeof(struct mctp_bus));
279 	mctp->busses[0].binding = binding;
280 	mctp->busses[0].eid = eid;
281 	binding->bus = &mctp->busses[0];
282 	binding->mctp = mctp;
283 	mctp->route_policy = ROUTE_ENDPOINT;
284 
285 	if (binding->start)
286 		binding->start(binding);
287 
288 	return 0;
289 }
290 
291 int mctp_bridge_busses(struct mctp *mctp,
292 		struct mctp_binding *b1, struct mctp_binding *b2)
293 {
294 	assert(mctp->n_busses == 0);
295 	mctp->busses = __mctp_alloc(2 * sizeof(struct mctp_bus));
296 	memset(mctp->busses, 0, 2 * sizeof(struct mctp_bus));
297 	mctp->n_busses = 2;
298 	mctp->busses[0].binding = b1;
299 	b1->bus = &mctp->busses[0];
300 	b1->mctp = mctp;
301 	mctp->busses[1].binding = b2;
302 	b2->bus = &mctp->busses[1];
303 	b2->mctp = mctp;
304 
305 	mctp->route_policy = ROUTE_BRIDGE;
306 
307 	if (b1->start)
308 		b1->start(b1);
309 
310 	if (b2->start)
311 		b2->start(b2);
312 
313 	return 0;
314 }
315 
316 static void mctp_rx(struct mctp *mctp, struct mctp_bus *bus,
317 		mctp_eid_t src, mctp_eid_t dest, void *buf, size_t len)
318 {
319 	if (mctp->route_policy == ROUTE_ENDPOINT &&
320 			dest == bus->eid && mctp->message_rx)
321 		mctp->message_rx(src, mctp->message_rx_data, buf, len);
322 
323 	if (mctp->route_policy == ROUTE_BRIDGE) {
324 		int i;
325 
326 		for (i = 0; i < mctp->n_busses; i++) {
327 			struct mctp_bus *dest_bus = &mctp->busses[i];
328 			if (dest_bus == bus)
329 				continue;
330 
331 			mctp_message_tx_on_bus(mctp, dest_bus,
332 					src, dest, buf, len);
333 		}
334 
335 	}
336 }
337 
338 void mctp_bus_rx(struct mctp_binding *binding, struct mctp_pktbuf *pkt)
339 {
340 	struct mctp_bus *bus = binding->bus;
341 	struct mctp *mctp = binding->mctp;
342 	uint8_t flags, exp_seq, seq, tag;
343 	struct mctp_msg_ctx *ctx;
344 	struct mctp_hdr *hdr;
345 	size_t len;
346 	void *p;
347 	int rc;
348 
349 	assert(bus);
350 
351 	hdr = mctp_pktbuf_hdr(pkt);
352 
353 	/* small optimisation: don't bother reassembly if we're going to
354 	 * drop the packet in mctp_rx anyway */
355 	if (mctp->route_policy == ROUTE_ENDPOINT && hdr->dest != bus->eid)
356 		goto out;
357 
358 	flags = hdr->flags_seq_tag & (MCTP_HDR_FLAG_SOM | MCTP_HDR_FLAG_EOM);
359 	tag = (hdr->flags_seq_tag >> MCTP_HDR_TAG_SHIFT) & MCTP_HDR_TAG_MASK;
360 	seq = (hdr->flags_seq_tag >> MCTP_HDR_SEQ_SHIFT) & MCTP_HDR_SEQ_MASK;
361 
362 	switch (flags) {
363 	case MCTP_HDR_FLAG_SOM | MCTP_HDR_FLAG_EOM:
364 		/* single-packet message - send straight up to rx function,
365 		 * no need to create a message context */
366 		len = pkt->end - pkt->mctp_hdr_off - sizeof(struct mctp_hdr);
367 		p = pkt->data + pkt->mctp_hdr_off + sizeof(struct mctp_hdr),
368 		mctp_rx(mctp, bus, hdr->src, hdr->dest, p, len);
369 		break;
370 
371 	case MCTP_HDR_FLAG_SOM:
372 		/* start of a new message - start the new context for
373 		 * future message reception. If an existing context is
374 		 * already present, drop it. */
375 		ctx = mctp_msg_ctx_lookup(mctp, hdr->src, hdr->dest, tag);
376 		if (ctx) {
377 			mctp_msg_ctx_reset(ctx);
378 		} else {
379 			ctx = mctp_msg_ctx_create(mctp,
380 					hdr->src, hdr->dest, tag);
381 		}
382 
383 		rc = mctp_msg_ctx_add_pkt(ctx, pkt);
384 		if (rc) {
385 			mctp_msg_ctx_drop(ctx);
386 		} else {
387 			ctx->last_seq = seq;
388 		}
389 
390 		break;
391 
392 	case MCTP_HDR_FLAG_EOM:
393 		ctx = mctp_msg_ctx_lookup(mctp, hdr->src, hdr->dest, tag);
394 		if (!ctx)
395 			goto out;
396 
397 		exp_seq = (ctx->last_seq + 1) % 4;
398 
399 		if (exp_seq != seq) {
400 			mctp_prdebug(
401 				"Sequence number %d does not match expected %d",
402 				seq, exp_seq);
403 			mctp_msg_ctx_drop(ctx);
404 			goto out;
405 		}
406 
407 		rc = mctp_msg_ctx_add_pkt(ctx, pkt);
408 		if (!rc)
409 			mctp_rx(mctp, bus, ctx->src, ctx->dest,
410 					ctx->buf, ctx->buf_size);
411 
412 		mctp_msg_ctx_drop(ctx);
413 		break;
414 
415 	case 0:
416 		/* Neither SOM nor EOM */
417 		ctx = mctp_msg_ctx_lookup(mctp, hdr->src,hdr->dest, tag);
418 		if (!ctx)
419 			goto out;
420 
421 		exp_seq = (ctx->last_seq + 1) % 4;
422 		if (exp_seq != seq) {
423 			mctp_prdebug(
424 				"Sequence number %d does not match expected %d",
425 				seq, exp_seq);
426 			mctp_msg_ctx_drop(ctx);
427 			goto out;
428 		}
429 
430 		rc = mctp_msg_ctx_add_pkt(ctx, pkt);
431 		if (rc) {
432 			mctp_msg_ctx_drop(ctx);
433 			goto out;
434 		}
435 		ctx->last_seq = seq;
436 
437 		break;
438 	}
439 out:
440 	mctp_pktbuf_free(pkt);
441 }
442 
443 static int mctp_packet_tx(struct mctp_bus *bus,
444 		struct mctp_pktbuf *pkt)
445 {
446 	if (!bus->tx_enabled)
447 		return -1;
448 
449 	return bus->binding->tx(bus->binding, pkt);
450 }
451 
452 static void mctp_send_tx_queue(struct mctp_bus *bus)
453 {
454 	struct mctp_pktbuf *pkt;
455 
456 	while ((pkt = bus->tx_queue_head)) {
457 		int rc;
458 
459 		rc = mctp_packet_tx(bus, pkt);
460 		if (rc)
461 			break;
462 
463 		bus->tx_queue_head = pkt->next;
464 		mctp_pktbuf_free(pkt);
465 	}
466 
467 	if (!bus->tx_queue_head)
468 		bus->tx_queue_tail = NULL;
469 
470 }
471 
472 void mctp_binding_set_tx_enabled(struct mctp_binding *binding, bool enable)
473 {
474 	struct mctp_bus *bus = binding->bus;
475 	bus->tx_enabled = enable;
476 	if (enable)
477 		mctp_send_tx_queue(bus);
478 }
479 
480 static int mctp_message_tx_on_bus(struct mctp *mctp, struct mctp_bus *bus,
481 		mctp_eid_t src, mctp_eid_t dest, void *msg, size_t msg_len)
482 {
483 	size_t max_payload_len, payload_len, p;
484 	struct mctp_pktbuf *pkt;
485 	struct mctp_hdr *hdr;
486 	int i;
487 
488 	max_payload_len = bus->binding->pkt_size - sizeof(*hdr);
489 
490 	mctp_prdebug("%s: Generating packets for transmission of %zu byte message from %hhu to %hhu",
491 		     __func__, msg_len, src, dest);
492 
493 	/* queue up packets, each of max MCTP_MTU size */
494 	for (p = 0, i = 0; p < msg_len; i++) {
495 		payload_len = msg_len - p;
496 		if (payload_len > max_payload_len)
497 			payload_len = max_payload_len;
498 
499 		pkt = mctp_pktbuf_alloc(bus->binding,
500 				payload_len + sizeof(*hdr));
501 		hdr = mctp_pktbuf_hdr(pkt);
502 
503 		/* todo: tags */
504 		hdr->ver = bus->binding->version & 0xf;
505 		hdr->dest = dest;
506 		hdr->src = src;
507 		hdr->flags_seq_tag = MCTP_HDR_FLAG_TO |
508 			(0 << MCTP_HDR_TAG_SHIFT);
509 
510 		if (i == 0)
511 			hdr->flags_seq_tag |= MCTP_HDR_FLAG_SOM;
512 		if (p + payload_len >= msg_len)
513 			hdr->flags_seq_tag |= MCTP_HDR_FLAG_EOM;
514 		hdr->flags_seq_tag |=
515 			(i & MCTP_HDR_SEQ_MASK) << MCTP_HDR_SEQ_SHIFT;
516 
517 		memcpy(mctp_pktbuf_data(pkt), msg + p, payload_len);
518 
519 		/* add to tx queue */
520 		if (bus->tx_queue_tail)
521 			bus->tx_queue_tail->next = pkt;
522 		else
523 			bus->tx_queue_head = pkt;
524 		bus->tx_queue_tail = pkt;
525 
526 		p += payload_len;
527 	}
528 
529 	mctp_prdebug("%s: Enqueued %d packets", __func__, i);
530 
531 	mctp_send_tx_queue(bus);
532 
533 	return 0;
534 }
535 
536 int mctp_message_tx(struct mctp *mctp, mctp_eid_t eid,
537 		void *msg, size_t msg_len)
538 {
539 	struct mctp_bus *bus;
540 
541 	bus = find_bus_for_eid(mctp, eid);
542 	return mctp_message_tx_on_bus(mctp, bus, bus->eid, eid, msg, msg_len);
543 }
544