common/mctp/mctp.c (408 lines of code) (raw):

#include "mctp.h" #include <logging/log.h> #include <stdint.h> #include <stdio.h> #include <stdlib.h> #include <string.h> #include <sys/printk.h> #include <zephyr.h> LOG_MODULE_REGISTER(mctp); typedef struct __attribute__((packed)) { uint8_t hdr_ver; uint8_t dest_ep; uint8_t src_ep; union { struct { uint8_t msg_tag : 3; uint8_t to : 1; uint8_t pkt_seq : 2; uint8_t eom : 1; uint8_t som : 1; }; uint8_t flags_seq_to_tag; }; } mctp_hdr; /* set thread name */ static uint8_t set_thread_name(mctp *mctp_inst) { if (!mctp_inst) return MCTP_ERROR; if (mctp_inst->medium_type <= MCTP_MEDIUM_TYPE_UNKNOWN || mctp_inst->medium_type >= MCTP_MEDIUM_TYPE_MAX) return MCTP_ERROR; if (mctp_inst->medium_type == MCTP_MEDIUM_TYPE_SMBUS) { mctp_smbus_conf *smbus_conf = (mctp_smbus_conf *)&mctp_inst->medium_conf; snprintf(mctp_inst->mctp_rx_task_name, sizeof(mctp_inst->mctp_rx_task_name), "mctprx_%02x_%02x_%02x", mctp_inst->medium_type, smbus_conf->bus, smbus_conf->addr); snprintf(mctp_inst->mctp_tx_task_name, sizeof(mctp_inst->mctp_tx_task_name), "mctptx_%02x_%02x_%02x", mctp_inst->medium_type, smbus_conf->bus, smbus_conf->addr); } return MCTP_SUCCESS; } /* init the medium related resources */ static uint8_t mctp_medium_init(mctp *mctp_inst, mctp_medium_conf medium_conf) { if (!mctp_inst) return MCTP_ERROR; uint8_t ret = MCTP_ERROR; switch (mctp_inst->medium_type) { case MCTP_MEDIUM_TYPE_SMBUS: ret = mctp_smbus_init(mctp_inst, medium_conf); break; default: break; } return ret; } static uint8_t mctp_medium_deinit(mctp *mctp_inst) { if (!mctp_inst) return MCTP_ERROR; switch (mctp_inst->medium_type) { case MCTP_MEDIUM_TYPE_SMBUS: mctp_smbus_deinit(mctp_inst); break; default: break; } return MCTP_SUCCESS; } static uint8_t bridge_msg(mctp *mctp_inst, uint8_t *buf, uint16_t len) { if (!mctp_inst || !buf || !len) return MCTP_ERROR; if (!mctp_inst->ep_resolve) return MCTP_ERROR; mctp *target_mctp = NULL; mctp_ext_params target_ext_params; memset(&target_ext_params, 0, sizeof(target_ext_params)); mctp_hdr *hdr = (mctp_hdr *)buf; uint8_t ret = mctp_inst->ep_resolve(hdr->dest_ep, (void **)&target_mctp, &target_ext_params); if (ret != MCTP_SUCCESS) { LOG_ERR("can't bridge endpoint %x", hdr->dest_ep); return MCTP_ERROR; } LOG_DBG("ret = %d, bridget msg to mctp = %p", ret, target_mctp); return mctp_bridge_msg(target_mctp, buf, len, target_ext_params); } static uint8_t mctp_pkt_assembling(mctp *mctp_inst, uint8_t *buf, uint16_t len) { if (!mctp_inst || !buf || !len) return MCTP_ERROR; mctp_hdr *hdr = (mctp_hdr *)buf; uint8_t **buf_p = &mctp_inst->temp_msg_buf[hdr->msg_tag].buf; uint16_t *offset_p = &mctp_inst->temp_msg_buf[hdr->msg_tag].offset; /* one packet message, do nothing */ if (hdr->som && hdr->eom) return MCTP_SUCCESS; /* first packet, allocate memory to hold data */ if (hdr->som && !hdr->eom) { if (*buf_p) free(*buf_p); *offset_p = 0; *buf_p = (uint8_t *)malloc(MSG_ASSEMBLY_BUF_SIZE); if (!*buf_p) { LOG_WRN("cannot create memory...\n"); return MCTP_ERROR; } memset(*buf_p, 0, MSG_ASSEMBLY_BUF_SIZE); } /* Appending other packet after the first packet */ memcpy(*buf_p + *offset_p, buf + sizeof(hdr), len - sizeof(hdr)); *offset_p += len - sizeof(hdr); return MCTP_SUCCESS; } /* mctp rx task */ static void mctp_rx_task(void *arg, void *dummy0, void *dummy1) { ARG_UNUSED(dummy0); ARG_UNUSED(dummy1); if (!arg) { LOG_WRN("mctp_rx_task without mctp_inst!"); return; } mctp *mctp_inst = (mctp *)arg; if (!mctp_inst->read_data) { LOG_WRN("mctp_rx_task without medium read function!"); return; } LOG_INF("mctp_rx_task start %p", mctp_inst); while (1) { uint8_t read_buf[256] = { 0 }; mctp_ext_params ext_params; uint8_t ret = MCTP_ERROR; memset(&ext_params, 0, sizeof(ext_params)); uint16_t read_len = mctp_inst->read_data(mctp_inst, read_buf, sizeof(read_buf), &ext_params); if (!read_len) continue; LOG_HEXDUMP_DBG(read_buf, read_len, "mctp receive data"); mctp_hdr *hdr = (mctp_hdr *)read_buf; LOG_DBG("dest_ep = %x, src_ep = %x, flags = %x\n", hdr->dest_ep, hdr->src_ep, hdr->flags_seq_to_tag); /* Set the tranport layer extra parameters */ ext_params.msg_tag = hdr->msg_tag; /* * The high-level application won't modify the tag_owner flag, change the * tag_owner for response if needs */ ext_params.tag_owner = 0; ext_params.ep = hdr->src_ep; if ((hdr->dest_ep != mctp_inst->endpoint) && (hdr->dest_ep != MCTP_NULL_EID)) { /* try to bridge this packet */ ret = bridge_msg(mctp_inst, read_buf, read_len); if (ret == MCTP_ERROR) LOG_WRN("Bridge to endpoint 0x%x failed ", hdr->dest_ep); continue; } /* handle this packet by self */ /* assembling the mctp message */ if (mctp_pkt_assembling(mctp_inst, read_buf, read_len) == MCTP_ERROR) LOG_WRN("Packet assemble failed "); /* if it is not last packet, waiting for the remain data */ if (!hdr->eom) continue; if (mctp_inst->rx_cb) { /* default process read data buffer directly */ uint8_t *p = read_buf + sizeof(hdr); uint16_t len = read_len - sizeof(hdr); /* this is assembly message */ if (mctp_inst->temp_msg_buf[hdr->msg_tag].buf) { p = mctp_inst->temp_msg_buf[hdr->msg_tag].buf; len = mctp_inst->temp_msg_buf[hdr->msg_tag].offset; LOG_HEXDUMP_DBG(p, len, "mctp assembly data"); } /* handle the mctp messsage */ mctp_inst->rx_cb(mctp_inst, p, len, ext_params); } if (mctp_inst->temp_msg_buf[hdr->msg_tag].buf) { free(mctp_inst->temp_msg_buf[hdr->msg_tag].buf); mctp_inst->temp_msg_buf[hdr->msg_tag].buf = NULL; mctp_inst->temp_msg_buf[hdr->msg_tag].offset = 0; } } } /* mctp tx task */ static void mctp_tx_task(void *arg, void *dummy0, void *dummy1) { ARG_UNUSED(dummy0); ARG_UNUSED(dummy1); if (!arg) { LOG_WRN("mctp_tx_task without mctp_inst!"); return; } mctp *mctp_inst = (mctp *)arg; if (!mctp_inst->write_data) { LOG_WRN("mctp_tx_task without medium write function!"); return; } LOG_INF("mctp_tx_task start %p ", mctp_inst); while (1) { mctp_tx_msg mctp_msg = { 0 }; int ret = k_msgq_get(&mctp_inst->mctp_tx_queue, &mctp_msg, K_FOREVER); if (ret) continue; if (!mctp_msg.buf) continue; if (!mctp_msg.len) { free(mctp_msg.buf); continue; } LOG_DBG("tx endpoint %x", mctp_msg.ext_params.ep); LOG_HEXDUMP_DBG(mctp_msg.buf, mctp_msg.len, "mctp tx task receive data"); /* * The bridge meesage already has the mctp transport header, and the bridge * message also doesn't need to split packet. */ if (mctp_msg.is_bridge_packet) { mctp_inst->write_data(mctp_inst, mctp_msg.buf, mctp_msg.len, mctp_msg.ext_params); free(mctp_msg.buf); continue; } /* Setup MCTP header and send to destination endpoint */ static uint8_t msg_tag; uint16_t max_msg_size = mctp_inst->max_msg_size; uint8_t i; uint8_t split_pkt_num = (mctp_msg.len / max_msg_size) + ((mctp_msg.len % max_msg_size) ? 1 : 0); LOG_DBG("mctp_msg.len = %d", mctp_msg.len); LOG_DBG("split_pkt_num = %d", split_pkt_num); for (i = 0; i < split_pkt_num; i++) { uint8_t buf[max_msg_size + MCTP_TRANSPORT_HEADER_SIZE]; mctp_hdr *hdr = (mctp_hdr *)buf; uint8_t cp_msg_size = max_msg_size; memset(buf, 0, sizeof(buf)); /* The first packet should set SOM */ if (!i) hdr->som = 1; /* The last packet should set EOM */ if (i == (split_pkt_num - 1)) { hdr->eom = 1; uint8_t remain = mctp_msg.len % max_msg_size; cp_msg_size = remain ? remain : max_msg_size; /* remain data */ } hdr->to = mctp_msg.ext_params.tag_owner; hdr->pkt_seq = i & MCTP_HDR_SEQ_MASK; /* * TODO: should avoid the msg_tag if there are pending mctp * response packets? * If the message is response, keep the original msg_tag of ext_params */ hdr->msg_tag = (hdr->to) ? (msg_tag & MCTP_HDR_TAG_MASK) : mctp_msg.ext_params.msg_tag; hdr->dest_ep = mctp_msg.ext_params.ep; hdr->src_ep = mctp_inst->endpoint; hdr->hdr_ver = MCTP_HDR_HDR_VER; LOG_DBG("i = %d, cp_msg_size = %d", i, cp_msg_size); LOG_DBG("hdr->flags_seq_to_tag = %x", hdr->flags_seq_to_tag); memcpy(buf + MCTP_TRANSPORT_HEADER_SIZE, mctp_msg.buf + i * max_msg_size, cp_msg_size); mctp_inst->write_data(mctp_inst, buf, cp_msg_size + MCTP_TRANSPORT_HEADER_SIZE, mctp_msg.ext_params); } free(mctp_msg.buf); /* Only request mctp message needs to increase msg_tag */ if (mctp_msg.ext_params.tag_owner) msg_tag++; } } /* mctp handle initial */ mctp *mctp_init(void) { mctp *mctp_inst = (mctp *)malloc(sizeof(*mctp_inst)); if (!mctp_inst) return NULL; memset(mctp_inst, 0, sizeof(*mctp_inst)); mctp_inst->medium_type = MCTP_MEDIUM_TYPE_UNKNOWN; mctp_inst->max_msg_size = MCTP_DEFAULT_MSG_MAX_SIZE; mctp_inst->endpoint = MCTP_DEFAULT_ENDPOINT; LOG_DBG("mctp_inst = %p", mctp_inst); return mctp_inst; } /* mctp handle deinitial */ uint8_t mctp_deinit(mctp *mctp_inst) { if (!mctp_inst) return MCTP_ERROR; LOG_DBG("mctp_inst = %p", mctp_inst); mctp_stop(mctp_inst); if (mctp_medium_deinit(mctp_inst) == MCTP_ERROR) LOG_WRN("mctp deinit failed "); free(mctp_inst); return MCTP_SUCCESS; } /* configure mctp handle with specific medium type */ uint8_t mctp_set_medium_configure(mctp *mctp_inst, MCTP_MEDIUM_TYPE medium_type, mctp_medium_conf medium_conf) { if (!mctp_inst) return MCTP_ERROR; if (medium_type <= MCTP_MEDIUM_TYPE_UNKNOWN || medium_type >= MCTP_MEDIUM_TYPE_MAX) return MCTP_ERROR; mctp_inst->medium_type = medium_type; if (mctp_medium_init(mctp_inst, medium_conf) == MCTP_ERROR) goto error; return MCTP_SUCCESS; error: if (mctp_medium_deinit(mctp_inst) == MCTP_ERROR) LOG_WRN("mctp deinit failed "); mctp_inst->medium_type = MCTP_MEDIUM_TYPE_UNKNOWN; return MCTP_ERROR; } uint8_t mctp_get_medium_configure(mctp *mctp_inst, MCTP_MEDIUM_TYPE *medium_type, mctp_medium_conf *medium_conf) { if (!mctp_inst || !medium_type || !medium_conf) return MCTP_ERROR; *medium_type = mctp_inst->medium_type; *medium_conf = mctp_inst->medium_conf; return MCTP_SUCCESS; } uint8_t mctp_stop(mctp *mctp_inst) { if (!mctp_inst) return MCTP_ERROR; if (mctp_inst->mctp_rx_task_tid) { k_thread_abort(mctp_inst->mctp_rx_task_tid); mctp_inst->mctp_rx_task_tid = NULL; } if (mctp_inst->mctp_tx_task_tid) { k_thread_abort(mctp_inst->mctp_tx_task_tid); mctp_inst->mctp_tx_task_tid = NULL; } if (mctp_inst->mctp_tx_queue.buffer_start) { free(mctp_inst->mctp_tx_queue.buffer_start); mctp_inst->mctp_tx_queue.buffer_start = NULL; } mctp_inst->is_servcie_start = 0; return MCTP_SUCCESS; } uint8_t mctp_start(mctp *mctp_inst) { if (!mctp_inst) return MCTP_ERROR; if (mctp_inst->is_servcie_start) { LOG_WRN("The mctp_inst is already start!"); return MCTP_ERROR; } set_thread_name(mctp_inst); uint8_t *msgq_buf = (uint8_t *)malloc(MCTP_TX_QUEUE_SIZE * sizeof(mctp_tx_msg)); if (!msgq_buf) { LOG_WRN("msgq alloc failed!!"); goto error; } k_msgq_init(&mctp_inst->mctp_tx_queue, msgq_buf, sizeof(mctp_tx_msg), MCTP_TX_QUEUE_SIZE); /* create rx service */ mctp_inst->mctp_rx_task_tid = k_thread_create(&mctp_inst->rx_task_thread_data, mctp_inst->rx_task_stack_area, K_KERNEL_STACK_SIZEOF(mctp_inst->rx_task_stack_area), mctp_rx_task, mctp_inst, NULL, NULL, K_PRIO_PREEMPT(10), 0, K_MSEC(1)); if (!mctp_inst->mctp_rx_task_tid) goto error; k_thread_name_set(mctp_inst->mctp_rx_task_tid, mctp_inst->mctp_rx_task_name); /* create tx service */ mctp_inst->mctp_tx_task_tid = k_thread_create(&mctp_inst->tx_task_thread_data, mctp_inst->tx_task_stack_area, K_KERNEL_STACK_SIZEOF(mctp_inst->tx_task_stack_area), mctp_tx_task, mctp_inst, NULL, NULL, K_PRIO_PREEMPT(10), 0, K_MSEC(1)); if (!mctp_inst->mctp_tx_task_tid) goto error; k_thread_name_set(mctp_inst->mctp_tx_task_tid, mctp_inst->mctp_tx_task_name); mctp_inst->is_servcie_start = 1; return MCTP_SUCCESS; error: LOG_ERR("mctp_start failed!!"); mctp_stop(mctp_inst); return MCTP_ERROR; } uint8_t mctp_bridge_msg(mctp *mctp_inst, uint8_t *buf, uint16_t len, mctp_ext_params ext_params) { if (!mctp_inst || !buf || !len) return MCTP_ERROR; if (!mctp_inst->is_servcie_start) { LOG_WRN("The mctp_inst isn't start service!"); return MCTP_ERROR; } mctp_tx_msg mctp_msg = { 0 }; mctp_msg.is_bridge_packet = 1; mctp_msg.len = len; mctp_msg.buf = (uint8_t *)malloc(len); if (!mctp_msg.buf) goto error; memcpy(mctp_msg.buf, buf, len); mctp_msg.ext_params = ext_params; int ret = k_msgq_put(&mctp_inst->mctp_tx_queue, &mctp_msg, K_NO_WAIT); if (!ret) return MCTP_SUCCESS; error: if (mctp_msg.buf) free(mctp_msg.buf); return MCTP_ERROR; } uint8_t mctp_send_msg(mctp *mctp_inst, uint8_t *buf, uint16_t len, mctp_ext_params ext_params) { if (!mctp_inst || !buf || !len) return MCTP_ERROR; if (!mctp_inst->is_servcie_start) { LOG_WRN("The mctp_inst isn't start service!"); return MCTP_ERROR; } mctp_tx_msg mctp_msg = { 0 }; mctp_msg.len = len; mctp_msg.buf = (uint8_t *)malloc(len); if (!mctp_msg.buf) { LOG_WRN("can't alloc buf!!"); goto error; } memcpy(mctp_msg.buf, buf, len); mctp_msg.ext_params = ext_params; int ret = k_msgq_put(&mctp_inst->mctp_tx_queue, &mctp_msg, K_NO_WAIT); if (!ret) return MCTP_SUCCESS; error: if (mctp_msg.buf) free(mctp_msg.buf); return MCTP_ERROR; } uint8_t mctp_reg_endpoint_resolve_func(mctp *mctp_inst, endpoint_resolve resolve_fn) { if (!mctp_inst || !resolve_fn) return MCTP_ERROR; mctp_inst->ep_resolve = resolve_fn; return MCTP_SUCCESS; } uint8_t mctp_reg_msg_rx_func(mctp *mctp_inst, mctp_fn_cb rx_cb) { if (!mctp_inst || !rx_cb) return MCTP_ERROR; mctp_inst->rx_cb = rx_cb; return MCTP_SUCCESS; }