blob: f8abbb1a1826588f2f5dbbbda466db97dbda3f46 [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;
204
205 /* @todo: finer-grained allocation, size limits */
206 if (!ctx->buf_alloc_size) {
207 new_alloc_size = 4096;
208 } else {
209 new_alloc_size = ctx->buf_alloc_size * 2;
210 }
211 ctx->buf = __mctp_realloc(ctx->buf, new_alloc_size);
212 ctx->buf_alloc_size = new_alloc_size;
213 }
214
215 memcpy(ctx->buf + ctx->buf_size, mctp_pktbuf_data(pkt), len);
216 ctx->buf_size += len;
217
218 return 0;
219}
220
221/* Core API functions */
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800222struct mctp *mctp_init(void)
223{
224 struct mctp *mctp;
225
226 mctp = __mctp_alloc(sizeof(*mctp));
227 memset(mctp, 0, sizeof(*mctp));
228
229 return mctp;
230}
231
232int mctp_set_rx_all(struct mctp *mctp, mctp_rx_fn fn, void *data)
233{
234 mctp->message_rx = fn;
235 mctp->message_rx_data = data;
236 return 0;
237}
238
239static struct mctp_bus *find_bus_for_eid(struct mctp *mctp,
240 mctp_eid_t dest __attribute__((unused)))
241{
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800242 /* for now, just use the first bus. For full routing support,
243 * we will need a table of neighbours */
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800244 return &mctp->busses[0];
245}
246
Jeremy Kerr7520cec2019-03-01 07:13:18 +0800247int mctp_register_bus(struct mctp *mctp,
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800248 struct mctp_binding *binding,
249 mctp_eid_t eid)
250{
Jeremy Kerr7520cec2019-03-01 07:13:18 +0800251 /* todo: multiple busses */
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800252 assert(mctp->n_busses == 0);
253 mctp->n_busses = 1;
254 mctp->busses = __mctp_alloc(sizeof(struct mctp_bus));
James Feist62d72362019-12-13 13:43:32 -0800255 memset(mctp->busses, 0, sizeof(struct mctp_bus));
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800256 mctp->busses[0].binding = binding;
257 mctp->busses[0].eid = eid;
Jeremy Kerr7520cec2019-03-01 07:13:18 +0800258 binding->bus = &mctp->busses[0];
Jeremy Kerr0a00dca2019-03-01 08:01:35 +0800259 binding->mctp = mctp;
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800260 mctp->route_policy = ROUTE_ENDPOINT;
Jeremy Kerr3b36d172019-09-04 11:56:09 +0800261
262 if (binding->start)
263 binding->start(binding);
264
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800265 return 0;
266}
267
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800268int mctp_bridge_busses(struct mctp *mctp,
269 struct mctp_binding *b1, struct mctp_binding *b2)
270{
271 assert(mctp->n_busses == 0);
272 mctp->busses = __mctp_alloc(2 * sizeof(struct mctp_bus));
James Feist62d72362019-12-13 13:43:32 -0800273 memset(mctp->busses, 0, 2 * sizeof(struct mctp_bus));
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800274 mctp->n_busses = 2;
275 mctp->busses[0].binding = b1;
276 b1->bus = &mctp->busses[0];
277 b1->mctp = mctp;
278 mctp->busses[1].binding = b2;
279 b2->bus = &mctp->busses[1];
280 b2->mctp = mctp;
281
282 mctp->route_policy = ROUTE_BRIDGE;
Jeremy Kerr3b36d172019-09-04 11:56:09 +0800283
284 if (b1->start)
285 b1->start(b1);
286
287 if (b2->start)
288 b2->start(b2);
289
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800290 return 0;
291}
292
293static void mctp_rx(struct mctp *mctp, struct mctp_bus *bus,
294 mctp_eid_t src, mctp_eid_t dest, void *buf, size_t len)
295{
296 if (mctp->route_policy == ROUTE_ENDPOINT &&
297 dest == bus->eid && mctp->message_rx)
298 mctp->message_rx(src, mctp->message_rx_data, buf, len);
299
300 if (mctp->route_policy == ROUTE_BRIDGE) {
301 int i;
302
303 for (i = 0; i < mctp->n_busses; i++) {
304 struct mctp_bus *dest_bus = &mctp->busses[i];
305 if (dest_bus == bus)
306 continue;
307
308 mctp_message_tx_on_bus(mctp, dest_bus,
309 src, dest, buf, len);
310 }
311
312 }
313}
314
Jeremy Kerr0a00dca2019-03-01 08:01:35 +0800315void mctp_bus_rx(struct mctp_binding *binding, struct mctp_pktbuf *pkt)
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800316{
Jeremy Kerr7520cec2019-03-01 07:13:18 +0800317 struct mctp_bus *bus = binding->bus;
Jeremy Kerr0a00dca2019-03-01 08:01:35 +0800318 struct mctp *mctp = binding->mctp;
Ed Tanousc2def9f2019-02-21 08:33:08 -0800319 uint8_t flags, exp_seq, seq, tag;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800320 struct mctp_msg_ctx *ctx;
321 struct mctp_hdr *hdr;
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800322 size_t len;
323 void *p;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800324 int rc;
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800325
Jeremy Kerr7520cec2019-03-01 07:13:18 +0800326 assert(bus);
327
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800328 hdr = mctp_pktbuf_hdr(pkt);
329
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800330 /* small optimisation: don't bother reassembly if we're going to
331 * drop the packet in mctp_rx anyway */
332 if (mctp->route_policy == ROUTE_ENDPOINT && hdr->dest != bus->eid)
Jeremy Kerrc1693af2019-08-05 14:30:59 +0800333 goto out;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800334
335 flags = hdr->flags_seq_tag & (MCTP_HDR_FLAG_SOM | MCTP_HDR_FLAG_EOM);
336 tag = (hdr->flags_seq_tag >> MCTP_HDR_TAG_SHIFT) & MCTP_HDR_TAG_MASK;
337 seq = (hdr->flags_seq_tag >> MCTP_HDR_SEQ_SHIFT) & MCTP_HDR_SEQ_MASK;
338
339 switch (flags) {
340 case MCTP_HDR_FLAG_SOM | MCTP_HDR_FLAG_EOM:
341 /* single-packet message - send straight up to rx function,
342 * no need to create a message context */
343 len = pkt->end - pkt->mctp_hdr_off - sizeof(struct mctp_hdr);
344 p = pkt->data + pkt->mctp_hdr_off + sizeof(struct mctp_hdr),
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800345 mctp_rx(mctp, bus, hdr->src, hdr->dest, p, len);
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800346 break;
347
348 case MCTP_HDR_FLAG_SOM:
349 /* start of a new message - start the new context for
350 * future message reception. If an existing context is
351 * already present, drop it. */
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800352 ctx = mctp_msg_ctx_lookup(mctp, hdr->src, hdr->dest, tag);
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800353 if (ctx) {
354 mctp_msg_ctx_reset(ctx);
355 } else {
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800356 ctx = mctp_msg_ctx_create(mctp,
357 hdr->src, hdr->dest, tag);
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800358 }
359
360 rc = mctp_msg_ctx_add_pkt(ctx, pkt);
361 if (rc) {
362 mctp_msg_ctx_drop(ctx);
363 } else {
364 ctx->last_seq = seq;
365 }
366
367 break;
368
369 case MCTP_HDR_FLAG_EOM:
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800370 ctx = mctp_msg_ctx_lookup(mctp, hdr->src, hdr->dest, tag);
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800371 if (!ctx)
Jeremy Kerrc1693af2019-08-05 14:30:59 +0800372 goto out;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800373
Ed Tanousc2def9f2019-02-21 08:33:08 -0800374 exp_seq = (ctx->last_seq + 1) % 4;
375
376 if (exp_seq != seq) {
377 mctp_prdebug(
378 "Sequence number %d does not match expected %d",
379 seq, exp_seq);
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800380 mctp_msg_ctx_drop(ctx);
Jeremy Kerrc1693af2019-08-05 14:30:59 +0800381 goto out;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800382 }
383
384 rc = mctp_msg_ctx_add_pkt(ctx, pkt);
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800385 if (!rc)
386 mctp_rx(mctp, bus, ctx->src, ctx->dest,
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800387 ctx->buf, ctx->buf_size);
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800388
389 mctp_msg_ctx_drop(ctx);
390 break;
Ed Tanousc2def9f2019-02-21 08:33:08 -0800391
392 case 0:
393 /* Neither SOM nor EOM */
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800394 ctx = mctp_msg_ctx_lookup(mctp, hdr->src,hdr->dest, tag);
Ed Tanousc2def9f2019-02-21 08:33:08 -0800395 if (!ctx)
Jeremy Kerrc1693af2019-08-05 14:30:59 +0800396 goto out;
Ed Tanousc2def9f2019-02-21 08:33:08 -0800397
398 exp_seq = (ctx->last_seq + 1) % 4;
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);
Jeremy Kerrc1693af2019-08-05 14:30:59 +0800404 goto out;
Ed Tanousc2def9f2019-02-21 08:33:08 -0800405 }
406
407 rc = mctp_msg_ctx_add_pkt(ctx, pkt);
408 if (rc) {
409 mctp_msg_ctx_drop(ctx);
Jeremy Kerrc1693af2019-08-05 14:30:59 +0800410 goto out;
Ed Tanousc2def9f2019-02-21 08:33:08 -0800411 }
412 ctx->last_seq = seq;
413
414 break;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800415 }
Jeremy Kerrc1693af2019-08-05 14:30:59 +0800416out:
417 mctp_pktbuf_free(pkt);
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800418}
419
Jeremy Kerr0a00dca2019-03-01 08:01:35 +0800420static int mctp_packet_tx(struct mctp_bus *bus,
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800421 struct mctp_pktbuf *pkt)
422{
Jeremy Kerr1cd31182019-02-27 18:01:00 +0800423 if (!bus->tx_enabled)
424 return -1;
425
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800426 return bus->binding->tx(bus->binding, pkt);
427}
428
Jeremy Kerrcc2458d2019-03-01 08:23:33 +0800429static void mctp_send_tx_queue(struct mctp_bus *bus)
Jeremy Kerr1cd31182019-02-27 18:01:00 +0800430{
431 struct mctp_pktbuf *pkt;
432
Jeremy Kerrcc2458d2019-03-01 08:23:33 +0800433 while ((pkt = bus->tx_queue_head)) {
Jeremy Kerr1cd31182019-02-27 18:01:00 +0800434 int rc;
435
436 rc = mctp_packet_tx(bus, pkt);
437 if (rc)
438 break;
439
Jeremy Kerrcc2458d2019-03-01 08:23:33 +0800440 bus->tx_queue_head = pkt->next;
Jeremy Kerr1cd31182019-02-27 18:01:00 +0800441 mctp_pktbuf_free(pkt);
442 }
443
Jeremy Kerrcc2458d2019-03-01 08:23:33 +0800444 if (!bus->tx_queue_head)
445 bus->tx_queue_tail = NULL;
Jeremy Kerr1cd31182019-02-27 18:01:00 +0800446
447}
448
449void mctp_binding_set_tx_enabled(struct mctp_binding *binding, bool enable)
450{
451 struct mctp_bus *bus = binding->bus;
452 bus->tx_enabled = enable;
453 if (enable)
Jeremy Kerrcc2458d2019-03-01 08:23:33 +0800454 mctp_send_tx_queue(bus);
Jeremy Kerr1cd31182019-02-27 18:01:00 +0800455}
456
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800457static int mctp_message_tx_on_bus(struct mctp *mctp, struct mctp_bus *bus,
458 mctp_eid_t src, mctp_eid_t dest, void *msg, size_t msg_len)
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800459{
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +0800460 size_t max_payload_len, payload_len, p;
Jeremy Kerr1cd31182019-02-27 18:01:00 +0800461 struct mctp_pktbuf *pkt;
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800462 struct mctp_hdr *hdr;
Jeremy Kerrc855d7b2019-08-01 21:18:09 +0800463 int i;
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800464
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +0800465 max_payload_len = bus->binding->pkt_size - sizeof(*hdr);
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800466
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800467 /* queue up packets, each of max MCTP_MTU size */
Jeremy Kerrc855d7b2019-08-01 21:18:09 +0800468 for (p = 0, i = 0; p < msg_len; i++) {
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +0800469 payload_len = msg_len - p;
470 if (payload_len > max_payload_len)
471 payload_len = max_payload_len;
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800472
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800473 pkt = mctp_pktbuf_alloc(bus->binding,
474 payload_len + sizeof(*hdr));
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800475 hdr = mctp_pktbuf_hdr(pkt);
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800476
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800477 /* todo: tags */
478 hdr->ver = bus->binding->version & 0xf;
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800479 hdr->dest = dest;
480 hdr->src = src;
Jeremy Kerrc855d7b2019-08-01 21:18:09 +0800481 hdr->flags_seq_tag = MCTP_HDR_FLAG_TO |
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800482 (0 << MCTP_HDR_TAG_SHIFT);
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800483
Jeremy Kerrc855d7b2019-08-01 21:18:09 +0800484 if (i == 0)
485 hdr->flags_seq_tag |= MCTP_HDR_FLAG_SOM;
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +0800486 if (p + payload_len >= msg_len)
Jeremy Kerrc855d7b2019-08-01 21:18:09 +0800487 hdr->flags_seq_tag |= MCTP_HDR_FLAG_EOM;
488 hdr->flags_seq_tag |=
489 (i & MCTP_HDR_SEQ_MASK) << MCTP_HDR_SEQ_SHIFT;
490
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +0800491 memcpy(mctp_pktbuf_data(pkt), msg + p, payload_len);
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800492
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800493 /* add to tx queue */
Jeremy Kerrcc2458d2019-03-01 08:23:33 +0800494 if (bus->tx_queue_tail)
495 bus->tx_queue_tail->next = pkt;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800496 else
Jeremy Kerrcc2458d2019-03-01 08:23:33 +0800497 bus->tx_queue_head = pkt;
498 bus->tx_queue_tail = pkt;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800499
Jeremy Kerrdf15f7e2019-08-05 15:41:19 +0800500 p += payload_len;
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800501 }
502
Jeremy Kerrcc2458d2019-03-01 08:23:33 +0800503 mctp_send_tx_queue(bus);
Jeremy Kerr24db71f2019-02-07 21:37:35 +0800504
505 return 0;
Jeremy Kerr4cdc2002019-02-07 16:49:12 +0800506}
Jeremy Kerr1a4ec3c2019-09-03 11:01:50 +0800507
508int mctp_message_tx(struct mctp *mctp, mctp_eid_t eid,
509 void *msg, size_t msg_len)
510{
511 struct mctp_bus *bus;
512
513 bus = find_bus_for_eid(mctp, eid);
514 return mctp_message_tx_on_bus(mctp, bus, bus->eid, eid, msg, msg_len);
515}