blob: 52961fd604f93d9c786350206538e727625274f9 [file] [log] [blame]
Jeremy Kerr3d36ee22019-05-30 11:15:37 +08001/* SPDX-License-Identifier: Apache-2.0 OR GPL-2.0-or-later */
Jeremy Kerr4cdc2002019-02-07 16:49:12 +08002
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
20struct mctp_bus {
21 mctp_eid_t eid;
22 struct mctp_binding *binding;
Jeremy Kerr1cd31182019-02-27 18:01:00 +080023 bool tx_enabled;
Jeremy Kerr4cdc2002019-02-07 16:49:12 +080024
Jeremy Kerrcc2458d2019-03-01 08:23:33 +080025 struct mctp_pktbuf *tx_queue_head;
26 struct mctp_pktbuf *tx_queue_tail;
27
Jeremy Kerr4cdc2002019-02-07 16:49:12 +080028 /* todo: routing */
29};
30
Jeremy Kerr24db71f2019-02-07 21:37:35 +080031struct mctp_msg_ctx {
32 uint8_t src;
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +080033 uint8_t dest;
Jeremy Kerr24db71f2019-02-07 21:37:35 +080034 uint8_t tag;
35 uint8_t last_seq;
36 void *buf;
37 size_t buf_size;
38 size_t buf_alloc_size;
39};
40
Jeremy Kerr4cdc2002019-02-07 16:49:12 +080041struct mctp {
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +080042 int n_busses;
43 struct mctp_bus *busses;
Jeremy Kerr4cdc2002019-02-07 16:49:12 +080044
Jeremy Kerr4cdc2002019-02-07 16:49:12 +080045 /* Message RX callback */
46 mctp_rx_fn message_rx;
47 void *message_rx_data;
Jeremy Kerr24db71f2019-02-07 21:37:35 +080048
49 /* Message reassembly.
50 * @todo: flexible context count
51 */
52 struct mctp_msg_ctx msg_ctxs[16];
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +080053
54 enum {
55 ROUTE_ENDPOINT,
56 ROUTE_BRIDGE,
57 } route_policy;
Jeremy Kerr4cdc2002019-02-07 16:49:12 +080058};
59
60#ifndef BUILD_ASSERT
61#define BUILD_ASSERT(x) \
62 do { (void)sizeof(char[0-(!(x))]); } while (0)
63#endif
64
Jeremy Kerr24db71f2019-02-07 21:37:35 +080065#ifndef ARRAY_SIZE
66#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
67#endif
68
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +080069static 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
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +080072struct mctp_pktbuf *mctp_pktbuf_alloc(struct mctp_binding *binding, size_t len)
Jeremy Kerr4cdc2002019-02-07 16:49:12 +080073{
74 struct mctp_pktbuf *buf;
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +080075 size_t size;
Jeremy Kerr4cdc2002019-02-07 16:49:12 +080076
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +080077 size = binding->pkt_size + binding->pkt_pad;
Jeremy Kerr4cdc2002019-02-07 16:49:12 +080078
79 /* todo: pools */
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +080080 buf = __mctp_alloc(sizeof(*buf) + size);
Jeremy Kerr4cdc2002019-02-07 16:49:12 +080081
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +080082 buf->size = size;
83 buf->start = binding->pkt_pad;
Jeremy Kerr4cdc2002019-02-07 16:49:12 +080084 buf->end = buf->start + len;
85 buf->mctp_hdr_off = buf->start;
Jeremy Kerrdd109f12019-04-04 11:46:49 +080086 buf->next = NULL;
Jeremy Kerr4cdc2002019-02-07 16:49:12 +080087
88 return buf;
89}
90
91void mctp_pktbuf_free(struct mctp_pktbuf *pkt)
92{
93 __mctp_free(pkt);
94}
95
96struct mctp_hdr *mctp_pktbuf_hdr(struct mctp_pktbuf *pkt)
97{
98 return (void *)pkt->data + pkt->mctp_hdr_off;
99}
100
101void *mctp_pktbuf_data(struct mctp_pktbuf *pkt)
102{
103 return (void *)pkt->data + pkt->mctp_hdr_off + sizeof(struct mctp_hdr);
104}
105
106uint8_t mctp_pktbuf_size(struct mctp_pktbuf *pkt)
107{
108 return pkt->end - pkt->start;
109}
110
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +0800111void *mctp_pktbuf_alloc_start(struct mctp_pktbuf *pkt, size_t size)
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800112{
113 assert(size <= pkt->start);
114 pkt->start -= size;
115 return pkt->data + pkt->start;
116}
117
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +0800118void *mctp_pktbuf_alloc_end(struct mctp_pktbuf *pkt, size_t size)
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800119{
120 void *buf;
121
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +0800122 assert(size < (pkt->size - pkt->end));
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800123 buf = pkt->data + pkt->end;
124 pkt->end += size;
125 return buf;
126}
127
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +0800128int mctp_pktbuf_push(struct mctp_pktbuf *pkt, void *data, size_t len)
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800129{
130 void *p;
131
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +0800132 if (pkt->end + len > pkt->size)
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800133 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
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800143/* Message reassembly */
144static struct mctp_msg_ctx *mctp_msg_ctx_lookup(struct mctp *mctp,
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800145 uint8_t src, uint8_t dest, uint8_t tag)
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800146{
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];
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800153 if (ctx->src == src && ctx->dest == dest && ctx->tag == tag)
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800154 return ctx;
155 }
156
157 return NULL;
158}
159
160static struct mctp_msg_ctx *mctp_msg_ctx_create(struct mctp *mctp,
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800161 uint8_t src, uint8_t dest, uint8_t tag)
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800162{
Jeremy Kerr11a234e2019-02-27 17:59:53 +0800163 struct mctp_msg_ctx *ctx = NULL;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800164 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;
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800178 ctx->dest = dest;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800179 ctx->tag = tag;
Jeremy Kerr9a3da812019-08-02 15:57:53 +0800180 ctx->buf_size = 0;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800181
182 return ctx;
183}
184
185static void mctp_msg_ctx_drop(struct mctp_msg_ctx *ctx)
186{
187 ctx->src = 0;
188}
189
190static void mctp_msg_ctx_reset(struct mctp_msg_ctx *ctx)
191{
192 ctx->buf_size = 0;
193}
194
195static 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;
Andrew Jeffery00ecc6c2020-03-10 23:16:53 +1030204 void *lbuf;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800205
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 }
Andrew Jeffery00ecc6c2020-03-10 23:16:53 +1030212
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 }
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800221 }
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 */
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800230struct 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
240int mctp_set_rx_all(struct mctp *mctp, mctp_rx_fn fn, void *data)
241{
242 mctp->message_rx = fn;
243 mctp->message_rx_data = data;
244 return 0;
245}
246
247static struct mctp_bus *find_bus_for_eid(struct mctp *mctp,
248 mctp_eid_t dest __attribute__((unused)))
249{
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800250 /* for now, just use the first bus. For full routing support,
251 * we will need a table of neighbours */
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800252 return &mctp->busses[0];
253}
254
Jeremy Kerr7520cec2019-03-01 07:13:18 +0800255int mctp_register_bus(struct mctp *mctp,
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800256 struct mctp_binding *binding,
257 mctp_eid_t eid)
258{
Jeremy Kerr7520cec2019-03-01 07:13:18 +0800259 /* todo: multiple busses */
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800260 assert(mctp->n_busses == 0);
261 mctp->n_busses = 1;
262 mctp->busses = __mctp_alloc(sizeof(struct mctp_bus));
James Feist62d72362019-12-13 13:43:32 -0800263 memset(mctp->busses, 0, sizeof(struct mctp_bus));
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800264 mctp->busses[0].binding = binding;
265 mctp->busses[0].eid = eid;
Jeremy Kerr7520cec2019-03-01 07:13:18 +0800266 binding->bus = &mctp->busses[0];
Jeremy Kerr0a00dca2019-03-01 08:01:35 +0800267 binding->mctp = mctp;
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800268 mctp->route_policy = ROUTE_ENDPOINT;
Jeremy Kerr3b36d172019-09-04 11:56:09 +0800269
270 if (binding->start)
271 binding->start(binding);
272
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800273 return 0;
274}
275
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800276int mctp_bridge_busses(struct mctp *mctp,
277 struct mctp_binding *b1, struct mctp_binding *b2)
278{
279 assert(mctp->n_busses == 0);
280 mctp->busses = __mctp_alloc(2 * sizeof(struct mctp_bus));
James Feist62d72362019-12-13 13:43:32 -0800281 memset(mctp->busses, 0, 2 * sizeof(struct mctp_bus));
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800282 mctp->n_busses = 2;
283 mctp->busses[0].binding = b1;
284 b1->bus = &mctp->busses[0];
285 b1->mctp = mctp;
286 mctp->busses[1].binding = b2;
287 b2->bus = &mctp->busses[1];
288 b2->mctp = mctp;
289
290 mctp->route_policy = ROUTE_BRIDGE;
Jeremy Kerr3b36d172019-09-04 11:56:09 +0800291
292 if (b1->start)
293 b1->start(b1);
294
295 if (b2->start)
296 b2->start(b2);
297
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800298 return 0;
299}
300
301static void mctp_rx(struct mctp *mctp, struct mctp_bus *bus,
302 mctp_eid_t src, mctp_eid_t dest, void *buf, size_t len)
303{
304 if (mctp->route_policy == ROUTE_ENDPOINT &&
305 dest == bus->eid && mctp->message_rx)
306 mctp->message_rx(src, mctp->message_rx_data, buf, len);
307
308 if (mctp->route_policy == ROUTE_BRIDGE) {
309 int i;
310
311 for (i = 0; i < mctp->n_busses; i++) {
312 struct mctp_bus *dest_bus = &mctp->busses[i];
313 if (dest_bus == bus)
314 continue;
315
316 mctp_message_tx_on_bus(mctp, dest_bus,
317 src, dest, buf, len);
318 }
319
320 }
321}
322
Jeremy Kerr0a00dca2019-03-01 08:01:35 +0800323void mctp_bus_rx(struct mctp_binding *binding, struct mctp_pktbuf *pkt)
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800324{
Jeremy Kerr7520cec2019-03-01 07:13:18 +0800325 struct mctp_bus *bus = binding->bus;
Jeremy Kerr0a00dca2019-03-01 08:01:35 +0800326 struct mctp *mctp = binding->mctp;
Ed Tanousc2def9f2019-02-21 08:33:08 -0800327 uint8_t flags, exp_seq, seq, tag;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800328 struct mctp_msg_ctx *ctx;
329 struct mctp_hdr *hdr;
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800330 size_t len;
331 void *p;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800332 int rc;
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800333
Jeremy Kerr7520cec2019-03-01 07:13:18 +0800334 assert(bus);
335
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800336 hdr = mctp_pktbuf_hdr(pkt);
337
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800338 /* small optimisation: don't bother reassembly if we're going to
339 * drop the packet in mctp_rx anyway */
340 if (mctp->route_policy == ROUTE_ENDPOINT && hdr->dest != bus->eid)
Jeremy Kerrc1693af2019-08-05 14:30:59 +0800341 goto out;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800342
343 flags = hdr->flags_seq_tag & (MCTP_HDR_FLAG_SOM | MCTP_HDR_FLAG_EOM);
344 tag = (hdr->flags_seq_tag >> MCTP_HDR_TAG_SHIFT) & MCTP_HDR_TAG_MASK;
345 seq = (hdr->flags_seq_tag >> MCTP_HDR_SEQ_SHIFT) & MCTP_HDR_SEQ_MASK;
346
347 switch (flags) {
348 case MCTP_HDR_FLAG_SOM | MCTP_HDR_FLAG_EOM:
349 /* single-packet message - send straight up to rx function,
350 * no need to create a message context */
351 len = pkt->end - pkt->mctp_hdr_off - sizeof(struct mctp_hdr);
352 p = pkt->data + pkt->mctp_hdr_off + sizeof(struct mctp_hdr),
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800353 mctp_rx(mctp, bus, hdr->src, hdr->dest, p, len);
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800354 break;
355
356 case MCTP_HDR_FLAG_SOM:
357 /* start of a new message - start the new context for
358 * future message reception. If an existing context is
359 * already present, drop it. */
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800360 ctx = mctp_msg_ctx_lookup(mctp, hdr->src, hdr->dest, tag);
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800361 if (ctx) {
362 mctp_msg_ctx_reset(ctx);
363 } else {
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800364 ctx = mctp_msg_ctx_create(mctp,
365 hdr->src, hdr->dest, tag);
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800366 }
367
368 rc = mctp_msg_ctx_add_pkt(ctx, pkt);
369 if (rc) {
370 mctp_msg_ctx_drop(ctx);
371 } else {
372 ctx->last_seq = seq;
373 }
374
375 break;
376
377 case MCTP_HDR_FLAG_EOM:
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800378 ctx = mctp_msg_ctx_lookup(mctp, hdr->src, hdr->dest, tag);
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800379 if (!ctx)
Jeremy Kerrc1693af2019-08-05 14:30:59 +0800380 goto out;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800381
Ed Tanousc2def9f2019-02-21 08:33:08 -0800382 exp_seq = (ctx->last_seq + 1) % 4;
383
384 if (exp_seq != seq) {
385 mctp_prdebug(
386 "Sequence number %d does not match expected %d",
387 seq, exp_seq);
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800388 mctp_msg_ctx_drop(ctx);
Jeremy Kerrc1693af2019-08-05 14:30:59 +0800389 goto out;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800390 }
391
392 rc = mctp_msg_ctx_add_pkt(ctx, pkt);
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800393 if (!rc)
394 mctp_rx(mctp, bus, ctx->src, ctx->dest,
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800395 ctx->buf, ctx->buf_size);
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800396
397 mctp_msg_ctx_drop(ctx);
398 break;
Ed Tanousc2def9f2019-02-21 08:33:08 -0800399
400 case 0:
401 /* Neither SOM nor EOM */
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800402 ctx = mctp_msg_ctx_lookup(mctp, hdr->src,hdr->dest, tag);
Ed Tanousc2def9f2019-02-21 08:33:08 -0800403 if (!ctx)
Jeremy Kerrc1693af2019-08-05 14:30:59 +0800404 goto out;
Ed Tanousc2def9f2019-02-21 08:33:08 -0800405
406 exp_seq = (ctx->last_seq + 1) % 4;
407 if (exp_seq != seq) {
408 mctp_prdebug(
409 "Sequence number %d does not match expected %d",
410 seq, exp_seq);
411 mctp_msg_ctx_drop(ctx);
Jeremy Kerrc1693af2019-08-05 14:30:59 +0800412 goto out;
Ed Tanousc2def9f2019-02-21 08:33:08 -0800413 }
414
415 rc = mctp_msg_ctx_add_pkt(ctx, pkt);
416 if (rc) {
417 mctp_msg_ctx_drop(ctx);
Jeremy Kerrc1693af2019-08-05 14:30:59 +0800418 goto out;
Ed Tanousc2def9f2019-02-21 08:33:08 -0800419 }
420 ctx->last_seq = seq;
421
422 break;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800423 }
Jeremy Kerrc1693af2019-08-05 14:30:59 +0800424out:
425 mctp_pktbuf_free(pkt);
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800426}
427
Jeremy Kerr0a00dca2019-03-01 08:01:35 +0800428static int mctp_packet_tx(struct mctp_bus *bus,
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800429 struct mctp_pktbuf *pkt)
430{
Jeremy Kerr1cd31182019-02-27 18:01:00 +0800431 if (!bus->tx_enabled)
432 return -1;
433
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800434 return bus->binding->tx(bus->binding, pkt);
435}
436
Jeremy Kerrcc2458d2019-03-01 08:23:33 +0800437static void mctp_send_tx_queue(struct mctp_bus *bus)
Jeremy Kerr1cd31182019-02-27 18:01:00 +0800438{
439 struct mctp_pktbuf *pkt;
440
Jeremy Kerrcc2458d2019-03-01 08:23:33 +0800441 while ((pkt = bus->tx_queue_head)) {
Jeremy Kerr1cd31182019-02-27 18:01:00 +0800442 int rc;
443
444 rc = mctp_packet_tx(bus, pkt);
445 if (rc)
446 break;
447
Jeremy Kerrcc2458d2019-03-01 08:23:33 +0800448 bus->tx_queue_head = pkt->next;
Jeremy Kerr1cd31182019-02-27 18:01:00 +0800449 mctp_pktbuf_free(pkt);
450 }
451
Jeremy Kerrcc2458d2019-03-01 08:23:33 +0800452 if (!bus->tx_queue_head)
453 bus->tx_queue_tail = NULL;
Jeremy Kerr1cd31182019-02-27 18:01:00 +0800454
455}
456
457void mctp_binding_set_tx_enabled(struct mctp_binding *binding, bool enable)
458{
459 struct mctp_bus *bus = binding->bus;
460 bus->tx_enabled = enable;
461 if (enable)
Jeremy Kerrcc2458d2019-03-01 08:23:33 +0800462 mctp_send_tx_queue(bus);
Jeremy Kerr1cd31182019-02-27 18:01:00 +0800463}
464
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800465static int mctp_message_tx_on_bus(struct mctp *mctp, struct mctp_bus *bus,
466 mctp_eid_t src, mctp_eid_t dest, void *msg, size_t msg_len)
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800467{
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +0800468 size_t max_payload_len, payload_len, p;
Jeremy Kerr1cd31182019-02-27 18:01:00 +0800469 struct mctp_pktbuf *pkt;
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800470 struct mctp_hdr *hdr;
Jeremy Kerrc855d7b2019-08-01 21:18:09 +0800471 int i;
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800472
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +0800473 max_payload_len = bus->binding->pkt_size - sizeof(*hdr);
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800474
Andrew Jeffery298865f2020-02-06 11:51:29 +1030475 mctp_prdebug("%s: Generating packets for transmission of %zu byte message from %hhu to %hhu",
476 __func__, msg_len, src, dest);
477
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800478 /* queue up packets, each of max MCTP_MTU size */
Jeremy Kerrc855d7b2019-08-01 21:18:09 +0800479 for (p = 0, i = 0; p < msg_len; i++) {
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +0800480 payload_len = msg_len - p;
481 if (payload_len > max_payload_len)
482 payload_len = max_payload_len;
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800483
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800484 pkt = mctp_pktbuf_alloc(bus->binding,
485 payload_len + sizeof(*hdr));
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800486 hdr = mctp_pktbuf_hdr(pkt);
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800487
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800488 /* todo: tags */
489 hdr->ver = bus->binding->version & 0xf;
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800490 hdr->dest = dest;
491 hdr->src = src;
Jeremy Kerrc855d7b2019-08-01 21:18:09 +0800492 hdr->flags_seq_tag = MCTP_HDR_FLAG_TO |
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800493 (0 << MCTP_HDR_TAG_SHIFT);
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800494
Jeremy Kerrc855d7b2019-08-01 21:18:09 +0800495 if (i == 0)
496 hdr->flags_seq_tag |= MCTP_HDR_FLAG_SOM;
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +0800497 if (p + payload_len >= msg_len)
Jeremy Kerrc855d7b2019-08-01 21:18:09 +0800498 hdr->flags_seq_tag |= MCTP_HDR_FLAG_EOM;
499 hdr->flags_seq_tag |=
500 (i & MCTP_HDR_SEQ_MASK) << MCTP_HDR_SEQ_SHIFT;
501
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +0800502 memcpy(mctp_pktbuf_data(pkt), msg + p, payload_len);
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800503
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800504 /* add to tx queue */
Jeremy Kerrcc2458d2019-03-01 08:23:33 +0800505 if (bus->tx_queue_tail)
506 bus->tx_queue_tail->next = pkt;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800507 else
Jeremy Kerrcc2458d2019-03-01 08:23:33 +0800508 bus->tx_queue_head = pkt;
509 bus->tx_queue_tail = pkt;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800510
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +0800511 p += payload_len;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800512 }
513
Andrew Jeffery298865f2020-02-06 11:51:29 +1030514 mctp_prdebug("%s: Enqueued %d packets", __func__, i);
515
Jeremy Kerrcc2458d2019-03-01 08:23:33 +0800516 mctp_send_tx_queue(bus);
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800517
518 return 0;
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800519}
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800520
521int mctp_message_tx(struct mctp *mctp, mctp_eid_t eid,
522 void *msg, size_t msg_len)
523{
524 struct mctp_bus *bus;
525
526 bus = find_bus_for_eid(mctp, eid);
527 return mctp_message_tx_on_bus(mctp, bus, bus->eid, eid, msg, msg_len);
528}