diff options
Diffstat (limited to 'drivers/net/wwan/iosm/iosm_ipc_mux_codec.c')
| -rw-r--r-- | drivers/net/wwan/iosm/iosm_ipc_mux_codec.c | 742 |
1 files changed, 693 insertions, 49 deletions
diff --git a/drivers/net/wwan/iosm/iosm_ipc_mux_codec.c b/drivers/net/wwan/iosm/iosm_ipc_mux_codec.c index 40fb54a0513e..d41e373f9c0a 100644 --- a/drivers/net/wwan/iosm/iosm_ipc_mux_codec.c +++ b/drivers/net/wwan/iosm/iosm_ipc_mux_codec.c @@ -54,6 +54,49 @@ static int ipc_mux_acb_send(struct iosm_mux *ipc_mux, bool blocking) return 0; } +/* Initialize the command header. */ +static void ipc_mux_acb_init(struct iosm_mux *ipc_mux) +{ + struct mux_acb *acb = &ipc_mux->acb; + struct mux_acbh *header; + + header = (struct mux_acbh *)(acb->skb)->data; + header->block_length = cpu_to_le32(sizeof(struct mux_acbh)); + header->first_cmd_index = header->block_length; + header->signature = cpu_to_le32(IOSM_AGGR_MUX_SIG_ACBH); + header->sequence_nr = cpu_to_le16(ipc_mux->acb_tx_sequence_nr++); +} + +/* Add a command to the ACB. */ +static struct mux_cmdh *ipc_mux_acb_add_cmd(struct iosm_mux *ipc_mux, u32 cmd, + void *param, u32 param_size) +{ + struct mux_acbh *header; + struct mux_cmdh *cmdh; + struct mux_acb *acb; + + acb = &ipc_mux->acb; + header = (struct mux_acbh *)(acb->skb)->data; + cmdh = (struct mux_cmdh *) + ((acb->skb)->data + le32_to_cpu(header->block_length)); + + cmdh->signature = cpu_to_le32(MUX_SIG_CMDH); + cmdh->command_type = cpu_to_le32(cmd); + cmdh->if_id = acb->if_id; + + acb->cmd = cmd; + cmdh->cmd_len = cpu_to_le16(offsetof(struct mux_cmdh, param) + + param_size); + cmdh->transaction_id = cpu_to_le32(ipc_mux->tx_transaction_id++); + if (param) + memcpy(&cmdh->param, param, param_size); + + skb_put(acb->skb, le32_to_cpu(header->block_length) + + le16_to_cpu(cmdh->cmd_len)); + + return cmdh; +} + /* Prepare mux Command */ static struct mux_lite_cmdh *ipc_mux_lite_add_cmd(struct iosm_mux *ipc_mux, u32 cmd, struct mux_acb *acb, @@ -104,7 +147,7 @@ int ipc_mux_dl_acb_send_cmds(struct iosm_mux *ipc_mux, u32 cmd_type, u8 if_id, size_t res_size, bool blocking, bool respond) { struct mux_acb *acb = &ipc_mux->acb; - struct mux_lite_cmdh *ack_lite; + union mux_type_cmdh cmdh; int ret = 0; acb->if_id = if_id; @@ -112,11 +155,23 @@ int ipc_mux_dl_acb_send_cmds(struct iosm_mux *ipc_mux, u32 cmd_type, u8 if_id, if (ret) return ret; - ack_lite = ipc_mux_lite_add_cmd(ipc_mux, cmd_type, acb, param, - res_size); - if (respond) - ack_lite->transaction_id = cpu_to_le32(transaction_id); + if (ipc_mux->protocol == MUX_LITE) { + cmdh.ack_lite = ipc_mux_lite_add_cmd(ipc_mux, cmd_type, acb, + param, res_size); + if (respond) + cmdh.ack_lite->transaction_id = + cpu_to_le32(transaction_id); + } else { + /* Initialize the ACB header. */ + ipc_mux_acb_init(ipc_mux); + cmdh.ack_aggr = ipc_mux_acb_add_cmd(ipc_mux, cmd_type, param, + res_size); + + if (respond) + cmdh.ack_aggr->transaction_id = + cpu_to_le32(transaction_id); + } ret = ipc_mux_acb_send(ipc_mux, blocking); return ret; @@ -129,15 +184,17 @@ void ipc_mux_netif_tx_flowctrl(struct mux_session *session, int idx, bool on) } static int ipc_mux_dl_cmdresps_decode_process(struct iosm_mux *ipc_mux, - struct mux_lite_cmdh *cmdh) + union mux_cmd_param param, + __le32 command_type, u8 if_id, + __le32 transaction_id) { struct mux_acb *acb = &ipc_mux->acb; - switch (le32_to_cpu(cmdh->command_type)) { + switch (le32_to_cpu(command_type)) { case MUX_CMD_OPEN_SESSION_RESP: case MUX_CMD_CLOSE_SESSION_RESP: /* Resume the control application. */ - acb->got_param = cmdh->param; + acb->got_param = param; break; case MUX_LITE_CMD_FLOW_CTL_ACK: @@ -147,8 +204,16 @@ static int ipc_mux_dl_cmdresps_decode_process(struct iosm_mux *ipc_mux, if (ipc_mux->protocol != MUX_LITE) return -EINVAL; - dev_dbg(ipc_mux->dev, "if %u FLOW_CTL_ACK %u received", - cmdh->if_id, le32_to_cpu(cmdh->transaction_id)); + dev_dbg(ipc_mux->dev, "if_id %u FLOW_CTL_ACK %u received", + if_id, le32_to_cpu(transaction_id)); + break; + + case IOSM_AGGR_MUX_CMD_FLOW_CTL_ACK: + /* This command type is not expected as response for + * Lite version of the protocol. So return non-zero. + */ + if (ipc_mux->protocol == MUX_LITE) + return -EINVAL; break; default: @@ -156,38 +221,39 @@ static int ipc_mux_dl_cmdresps_decode_process(struct iosm_mux *ipc_mux, } acb->wanted_response = MUX_CMD_INVALID; - acb->got_response = le32_to_cpu(cmdh->command_type); + acb->got_response = le32_to_cpu(command_type); complete(&ipc_mux->channel->ul_sem); return 0; } -static int ipc_mux_dl_dlcmds_decode_process(struct iosm_mux *ipc_mux, - struct mux_lite_cmdh *cmdh) +static int ipc_mux_dl_cmds_decode_process(struct iosm_mux *ipc_mux, + union mux_cmd_param *param, + __le32 command_type, u8 if_id, + __le16 cmd_len, int size) { - union mux_cmd_param *param = &cmdh->param; struct mux_session *session; - int new_size; + struct hrtimer *adb_timer; dev_dbg(ipc_mux->dev, "if_id[%d]: dlcmds decode process %d", - cmdh->if_id, le32_to_cpu(cmdh->command_type)); + if_id, le32_to_cpu(command_type)); - switch (le32_to_cpu(cmdh->command_type)) { + switch (le32_to_cpu(command_type)) { case MUX_LITE_CMD_FLOW_CTL: + case IOSM_AGGR_MUX_CMD_FLOW_CTL_DISABLE: - if (cmdh->if_id >= IPC_MEM_MUX_IP_SESSION_ENTRIES) { + if (if_id >= IPC_MEM_MUX_IP_SESSION_ENTRIES) { dev_err(ipc_mux->dev, "if_id [%d] not valid", - cmdh->if_id); + if_id); return -EINVAL; /* No session interface id. */ } - session = &ipc_mux->session[cmdh->if_id]; + session = &ipc_mux->session[if_id]; + adb_timer = &ipc_mux->imem->adb_timer; - new_size = offsetof(struct mux_lite_cmdh, param) + - sizeof(param->flow_ctl); if (param->flow_ctl.mask == cpu_to_le32(0xFFFFFFFF)) { /* Backward Compatibility */ - if (cmdh->cmd_len == cpu_to_le16(new_size)) + if (cmd_len == cpu_to_le16(size)) session->flow_ctl_mask = le32_to_cpu(param->flow_ctl.mask); else @@ -197,6 +263,16 @@ static int ipc_mux_dl_dlcmds_decode_process(struct iosm_mux *ipc_mux, * to limit uplink session queueing */ session->net_tx_stop = true; + + /* We have to call Finish ADB here. + * Otherwise any already queued data + * will be sent to CP when ADB is full + * for some other sessions. + */ + if (ipc_mux->protocol == MUX_AGGREGATION) { + ipc_mux_ul_adb_finish(ipc_mux); + ipc_imem_hrtimer_stop(adb_timer); + } /* Update the stats */ session->flow_ctl_en_cnt++; } else if (param->flow_ctl.mask == 0) { @@ -205,8 +281,10 @@ static int ipc_mux_dl_dlcmds_decode_process(struct iosm_mux *ipc_mux, * our internal Tx flag and enabling kernel * flow control */ + dev_dbg(ipc_mux->dev, "if_id[%u] flow_ctl mask 0x%08X", + if_id, le32_to_cpu(param->flow_ctl.mask)); /* Backward Compatibility */ - if (cmdh->cmd_len == cpu_to_le16(new_size)) + if (cmd_len == cpu_to_le16(size)) session->flow_ctl_mask = le32_to_cpu(param->flow_ctl.mask); else @@ -217,7 +295,10 @@ static int ipc_mux_dl_dlcmds_decode_process(struct iosm_mux *ipc_mux, break; } - dev_dbg(ipc_mux->dev, "if[%u] FLOW CTRL 0x%08X", cmdh->if_id, + ipc_mux->acc_adb_size = 0; + ipc_mux->acc_payload_size = 0; + + dev_dbg(ipc_mux->dev, "if_id[%u] FLOW CTRL 0x%08X", if_id, le32_to_cpu(param->flow_ctl.mask)); break; @@ -235,12 +316,20 @@ static void ipc_mux_dl_cmd_decode(struct iosm_mux *ipc_mux, struct sk_buff *skb) { struct mux_lite_cmdh *cmdh = (struct mux_lite_cmdh *)skb->data; __le32 trans_id = cmdh->transaction_id; + int size; - if (ipc_mux_dl_cmdresps_decode_process(ipc_mux, cmdh)) { + if (ipc_mux_dl_cmdresps_decode_process(ipc_mux, cmdh->param, + cmdh->command_type, cmdh->if_id, + cmdh->transaction_id)) { /* Unable to decode command response indicates the cmd_type * may be a command instead of response. So try to decoding it. */ - if (!ipc_mux_dl_dlcmds_decode_process(ipc_mux, cmdh)) { + size = offsetof(struct mux_lite_cmdh, param) + + sizeof(cmdh->param.flow_ctl); + if (!ipc_mux_dl_cmds_decode_process(ipc_mux, &cmdh->param, + cmdh->command_type, + cmdh->if_id, + cmdh->cmd_len, size)) { /* Decoded command may need a response. Give the * response according to the command type. */ @@ -349,7 +438,7 @@ static void ipc_mux_dl_adgh_decode(struct iosm_mux *ipc_mux, adgh = (struct mux_adgh *)block; - if (adgh->signature != cpu_to_le32(MUX_SIG_ADGH)) { + if (adgh->signature != cpu_to_le32(IOSM_AGGR_MUX_SIG_ADGH)) { dev_err(ipc_mux->dev, "invalid ADGH signature received"); return; } @@ -392,6 +481,192 @@ static void ipc_mux_dl_adgh_decode(struct iosm_mux *ipc_mux, ipc_mux->session[if_id].flush = 1; } +static void ipc_mux_dl_acbcmd_decode(struct iosm_mux *ipc_mux, + struct mux_cmdh *cmdh, int size) +{ + u32 link_st = IOSM_AGGR_MUX_CMD_LINK_STATUS_REPORT_RESP; + u32 fctl_dis = IOSM_AGGR_MUX_CMD_FLOW_CTL_DISABLE; + u32 fctl_ena = IOSM_AGGR_MUX_CMD_FLOW_CTL_ENABLE; + u32 fctl_ack = IOSM_AGGR_MUX_CMD_FLOW_CTL_ACK; + union mux_cmd_param *cmd_p = NULL; + u32 cmd = link_st; + u32 trans_id; + + if (!ipc_mux_dl_cmds_decode_process(ipc_mux, &cmdh->param, + cmdh->command_type, cmdh->if_id, + cmdh->cmd_len, size)) { + size = 0; + if (cmdh->command_type == cpu_to_le32(link_st)) { + cmd_p = &cmdh->param; + cmd_p->link_status_resp.response = MUX_CMD_RESP_SUCCESS; + } else if ((cmdh->command_type == cpu_to_le32(fctl_ena)) || + (cmdh->command_type == cpu_to_le32(fctl_dis))) { + cmd = fctl_ack; + } else { + return; + } + trans_id = le32_to_cpu(cmdh->transaction_id); + ipc_mux_dl_acb_send_cmds(ipc_mux, cmd, cmdh->if_id, + trans_id, cmd_p, size, false, true); + } +} + +/* Decode an aggregated command block. */ +static void ipc_mux_dl_acb_decode(struct iosm_mux *ipc_mux, struct sk_buff *skb) +{ + struct mux_acbh *acbh; + struct mux_cmdh *cmdh; + u32 next_cmd_index; + u8 *block; + int size; + + acbh = (struct mux_acbh *)(skb->data); + block = (u8 *)(skb->data); + + next_cmd_index = le32_to_cpu(acbh->first_cmd_index); + next_cmd_index = array_index_nospec(next_cmd_index, + sizeof(struct mux_cmdh)); + + while (next_cmd_index != 0) { + cmdh = (struct mux_cmdh *)&block[next_cmd_index]; + next_cmd_index = le32_to_cpu(cmdh->next_cmd_index); + if (ipc_mux_dl_cmdresps_decode_process(ipc_mux, cmdh->param, + cmdh->command_type, + cmdh->if_id, + cmdh->transaction_id)) { + size = offsetof(struct mux_cmdh, param) + + sizeof(cmdh->param.flow_ctl); + ipc_mux_dl_acbcmd_decode(ipc_mux, cmdh, size); + } + } +} + +/* process datagram */ +static int mux_dl_process_dg(struct iosm_mux *ipc_mux, struct mux_adbh *adbh, + struct mux_adth_dg *dg, struct sk_buff *skb, + int if_id, int nr_of_dg) +{ + u32 dl_head_pad_len = ipc_mux->session[if_id].dl_head_pad_len; + u32 packet_offset, i, rc; + + for (i = 0; i < nr_of_dg; i++, dg++) { + if (le32_to_cpu(dg->datagram_index) + < sizeof(struct mux_adbh)) + goto dg_error; + + /* Is the packet inside of the ADB */ + if (le32_to_cpu(dg->datagram_index) >= + le32_to_cpu(adbh->block_length)) { + goto dg_error; + } else { + packet_offset = + le32_to_cpu(dg->datagram_index) + + dl_head_pad_len; + /* Pass the packet to the netif layer. */ + rc = ipc_mux_net_receive(ipc_mux, if_id, ipc_mux->wwan, + packet_offset, + dg->service_class, + skb); + if (rc) + goto dg_error; + } + } + return 0; +dg_error: + return -1; +} + +/* Decode an aggregated data block. */ +static void mux_dl_adb_decode(struct iosm_mux *ipc_mux, + struct sk_buff *skb) +{ + struct mux_adth_dg *dg; + struct iosm_wwan *wwan; + struct mux_adbh *adbh; + struct mux_adth *adth; + int nr_of_dg, if_id; + u32 adth_index; + u8 *block; + + block = skb->data; + adbh = (struct mux_adbh *)block; + + /* Process the aggregated datagram tables. */ + adth_index = le32_to_cpu(adbh->first_table_index); + + /* Has CP sent an empty ADB ? */ + if (adth_index < 1) { + dev_err(ipc_mux->dev, "unexpected empty ADB"); + goto adb_decode_err; + } + + /* Loop through mixed session tables. */ + while (adth_index) { + /* Get the reference to the table header. */ + adth = (struct mux_adth *)(block + adth_index); + + /* Get the interface id and map it to the netif id. */ + if_id = adth->if_id; + if (if_id >= IPC_MEM_MUX_IP_SESSION_ENTRIES) + goto adb_decode_err; + + if_id = array_index_nospec(if_id, + IPC_MEM_MUX_IP_SESSION_ENTRIES); + + /* Is the session active ? */ + wwan = ipc_mux->session[if_id].wwan; + if (!wwan) + goto adb_decode_err; + + /* Consistency checks for aggregated datagram table. */ + if (adth->signature != cpu_to_le32(IOSM_AGGR_MUX_SIG_ADTH)) + goto adb_decode_err; + + if (le16_to_cpu(adth->table_length) < (sizeof(struct mux_adth) - + sizeof(struct mux_adth_dg))) + goto adb_decode_err; + + /* Calculate the number of datagrams. */ + nr_of_dg = (le16_to_cpu(adth->table_length) - + sizeof(struct mux_adth) + + sizeof(struct mux_adth_dg)) / + sizeof(struct mux_adth_dg); + + /* Is the datagram table empty ? */ + if (nr_of_dg < 1) { + dev_err(ipc_mux->dev, + "adthidx=%u,nr_of_dg=%d,next_tblidx=%u", + adth_index, nr_of_dg, + le32_to_cpu(adth->next_table_index)); + + /* Move to the next aggregated datagram table. */ + adth_index = le32_to_cpu(adth->next_table_index); + continue; + } + + /* New aggregated datagram table. */ + dg = &adth->dg; + if (mux_dl_process_dg(ipc_mux, adbh, dg, skb, if_id, + nr_of_dg) < 0) + goto adb_decode_err; + + /* mark session for final flush */ + ipc_mux->session[if_id].flush = 1; + + /* Move to the next aggregated datagram table. */ + adth_index = le32_to_cpu(adth->next_table_index); + } + +adb_decode_err: + return; +} + +/** + * ipc_mux_dl_decode - Route the DL packet through the IP MUX layer + * depending on Header. + * @ipc_mux: Pointer to MUX data-struct + * @skb: Pointer to ipc_skb. + */ void ipc_mux_dl_decode(struct iosm_mux *ipc_mux, struct sk_buff *skb) { u32 signature; @@ -403,14 +678,18 @@ void ipc_mux_dl_decode(struct iosm_mux *ipc_mux, struct sk_buff *skb) signature = le32_to_cpup((__le32 *)skb->data); switch (signature) { - case MUX_SIG_ADGH: + case IOSM_AGGR_MUX_SIG_ADBH: /* Aggregated Data Block Header */ + mux_dl_adb_decode(ipc_mux, skb); + break; + case IOSM_AGGR_MUX_SIG_ADGH: ipc_mux_dl_adgh_decode(ipc_mux, skb); break; - case MUX_SIG_FCTH: ipc_mux_dl_fcth_decode(ipc_mux, skb->data); break; - + case IOSM_AGGR_MUX_SIG_ACBH: /* Aggregated Command Block Header */ + ipc_mux_dl_acb_decode(ipc_mux, skb); + break; case MUX_SIG_CMDH: ipc_mux_dl_cmd_decode(ipc_mux, skb); break; @@ -427,7 +706,10 @@ static int ipc_mux_ul_skb_alloc(struct iosm_mux *ipc_mux, { /* Take the first element of the free list. */ struct sk_buff *skb = skb_dequeue(&ul_adb->free_list); + u32 no_if = IPC_MEM_MUX_IP_SESSION_ENTRIES; + u32 *next_tb_id; int qlt_size; + u32 if_id; if (!skb) return -EBUSY; /* Wait for a free ADB skb. */ @@ -436,7 +718,37 @@ static int ipc_mux_ul_skb_alloc(struct iosm_mux *ipc_mux, IPC_CB(skb)->op_type = (u8)UL_MUX_OP_ADB; switch (type) { - case MUX_SIG_ADGH: + case IOSM_AGGR_MUX_SIG_ADBH: + /* Save the ADB memory settings. */ + ul_adb->dest_skb = skb; + ul_adb->buf = skb->data; + ul_adb->size = IPC_MEM_MAX_ADB_BUF_SIZE; + + /* reset statistic counter */ + ul_adb->if_cnt = 0; + ul_adb->payload_size = 0; + ul_adb->dg_cnt_total = 0; + + /* Initialize the ADBH. */ + ul_adb->adbh = (struct mux_adbh *)ul_adb->buf; + memset(ul_adb->adbh, 0, sizeof(struct mux_adbh)); + ul_adb->adbh->signature = cpu_to_le32(IOSM_AGGR_MUX_SIG_ADBH); + ul_adb->adbh->block_length = + cpu_to_le32(sizeof(struct mux_adbh)); + next_tb_id = (unsigned int *)&ul_adb->adbh->first_table_index; + ul_adb->next_table_index = next_tb_id; + + /* Clear the local copy of DGs for new ADB */ + memset(ul_adb->dg, 0, sizeof(ul_adb->dg)); + + /* Clear the DG count and QLT updated status for new ADB */ + for (if_id = 0; if_id < no_if; if_id++) { + ul_adb->dg_count[if_id] = 0; + ul_adb->qlt_updated[if_id] = 0; + } + break; + + case IOSM_AGGR_MUX_SIG_ADGH: /* Save the ADB memory settings. */ ul_adb->dest_skb = skb; ul_adb->buf = skb->data; @@ -506,6 +818,94 @@ static void ipc_mux_ul_adgh_finish(struct iosm_mux *ipc_mux) str, bytes); } +static void ipc_mux_ul_encode_adth(struct iosm_mux *ipc_mux, + struct mux_adb *ul_adb, int *out_offset) +{ + int i, qlt_size, offset = *out_offset; + struct mux_qlth *p_adb_qlt; + struct mux_adth_dg *dg; + struct mux_adth *adth; + u16 adth_dg_size; + u32 *next_tb_id; + + qlt_size = offsetof(struct mux_qlth, ql) + + MUX_QUEUE_LEVEL * sizeof(struct mux_qlth_ql); + + for (i = 0; i < ipc_mux->nr_sessions; i++) { + if (ul_adb->dg_count[i] > 0) { + adth_dg_size = offsetof(struct mux_adth, dg) + + ul_adb->dg_count[i] * sizeof(*dg); + + *ul_adb->next_table_index = offset; + adth = (struct mux_adth *)&ul_adb->buf[offset]; + next_tb_id = (unsigned int *)&adth->next_table_index; + ul_adb->next_table_index = next_tb_id; + offset += adth_dg_size; + adth->signature = cpu_to_le32(IOSM_AGGR_MUX_SIG_ADTH); + adth->if_id = i; + adth->table_length = cpu_to_le16(adth_dg_size); + adth_dg_size -= offsetof(struct mux_adth, dg); + memcpy(&adth->dg, ul_adb->dg[i], adth_dg_size); + ul_adb->if_cnt++; + } + + if (ul_adb->qlt_updated[i]) { + *ul_adb->next_table_index = offset; + p_adb_qlt = (struct mux_qlth *)&ul_adb->buf[offset]; + ul_adb->next_table_index = + (u32 *)&p_adb_qlt->next_table_index; + memcpy(p_adb_qlt, ul_adb->pp_qlt[i], qlt_size); + offset += qlt_size; + } + } + *out_offset = offset; +} + +/** + * ipc_mux_ul_adb_finish - Add the TD of the aggregated session packets to TDR. + * @ipc_mux: Pointer to MUX data-struct. + */ +void ipc_mux_ul_adb_finish(struct iosm_mux *ipc_mux) +{ + bool ul_data_pend = false; + struct mux_adb *ul_adb; + unsigned long flags; + int offset; + + ul_adb = &ipc_mux->ul_adb; + if (!ul_adb->dest_skb) + return; + + offset = *ul_adb->next_table_index; + ipc_mux_ul_encode_adth(ipc_mux, ul_adb, &offset); + ul_adb->adbh->block_length = cpu_to_le32(offset); + + if (le32_to_cpu(ul_adb->adbh->block_length) > ul_adb->size) { + ul_adb->dest_skb = NULL; + return; + } + + *ul_adb->next_table_index = 0; + ul_adb->adbh->sequence_nr = cpu_to_le16(ipc_mux->adb_tx_sequence_nr++); + skb_put(ul_adb->dest_skb, le32_to_cpu(ul_adb->adbh->block_length)); + + spin_lock_irqsave(&(&ipc_mux->channel->ul_list)->lock, flags); + __skb_queue_tail(&ipc_mux->channel->ul_list, ul_adb->dest_skb); + spin_unlock_irqrestore(&(&ipc_mux->channel->ul_list)->lock, flags); + + ul_adb->dest_skb = NULL; + /* Updates the TDs with ul_list */ + ul_data_pend = ipc_imem_ul_write_td(ipc_mux->imem); + + /* Delay the doorbell irq */ + if (ul_data_pend) + ipc_imem_td_update_timer_start(ipc_mux->imem); + + ipc_mux->acc_adb_size += le32_to_cpu(ul_adb->adbh->block_length); + ipc_mux->acc_payload_size += ul_adb->payload_size; + ipc_mux->ul_data_pend_bytes += ul_adb->payload_size; +} + /* Allocates an ADB from the free list and initializes it with ADBH */ static bool ipc_mux_ul_adb_allocate(struct iosm_mux *ipc_mux, struct mux_adb *adb, int *size_needed, @@ -688,7 +1088,7 @@ static int ipc_mux_ul_adgh_encode(struct iosm_mux *ipc_mux, int session_id, while (nr_of_pkts > 0) { /* get destination skb allocated */ if (ipc_mux_ul_adb_allocate(ipc_mux, adb, &ipc_mux->size_needed, - MUX_SIG_ADGH)) { + IOSM_AGGR_MUX_SIG_ADGH)) { dev_err(ipc_mux->dev, "no reserved memory for ADGH"); return -ENOMEM; } @@ -720,7 +1120,7 @@ static int ipc_mux_ul_adgh_encode(struct iosm_mux *ipc_mux, int session_id, memcpy(adb->buf + offset + pad_len, src_skb->data, src_skb->len); - adb->adgh->signature = cpu_to_le32(MUX_SIG_ADGH); + adb->adgh->signature = cpu_to_le32(IOSM_AGGR_MUX_SIG_ADGH); adb->adgh->if_id = session_id; adb->adgh->length = cpu_to_le16(sizeof(struct mux_adgh) + pad_len + @@ -762,6 +1162,187 @@ static int ipc_mux_ul_adgh_encode(struct iosm_mux *ipc_mux, int session_id, return adb_updated; } +/** + * ipc_mux_ul_adb_update_ql - Adds Queue Level Table and Queue Level to ADB + * @ipc_mux: pointer to MUX instance data + * @p_adb: pointer to UL aggegated data block + * @session_id: session id + * @qlth_n_ql_size: Length (in bytes) of the datagram table + * @ul_list: pointer to skb buffer head + */ +void ipc_mux_ul_adb_update_ql(struct iosm_mux *ipc_mux, struct mux_adb *p_adb, + int session_id, int qlth_n_ql_size, + struct sk_buff_head *ul_list) +{ + int qlevel = ul_list->qlen; + struct mux_qlth *p_qlt; + + p_qlt = (struct mux_qlth *)p_adb->pp_qlt[session_id]; + + /* Initialize QLTH if not been done */ + if (p_adb->qlt_updated[session_id] == 0) { + p_qlt->signature = cpu_to_le32(MUX_SIG_QLTH); + p_qlt->if_id = session_id; + p_qlt->table_length = cpu_to_le16(qlth_n_ql_size); + p_qlt->reserved = 0; + p_qlt->reserved2 = 0; + } + + /* Update Queue Level information always */ + p_qlt->ql.nr_of_bytes = cpu_to_le32(qlevel); + p_adb->qlt_updated[session_id] = 1; +} + +/* Update the next table index. */ +static int mux_ul_dg_update_tbl_index(struct iosm_mux *ipc_mux, + int session_id, + struct sk_buff_head *ul_list, + struct mux_adth_dg *dg, + int aligned_size, + u32 qlth_n_ql_size, + struct mux_adb *adb, + struct sk_buff *src_skb) +{ + ipc_mux_ul_adb_update_ql(ipc_mux, adb, session_id, + qlth_n_ql_size, ul_list); + ipc_mux_ul_adb_finish(ipc_mux); + if (ipc_mux_ul_adb_allocate(ipc_mux, adb, &ipc_mux->size_needed, + IOSM_AGGR_MUX_SIG_ADBH)) { + dev_kfree_skb(src_skb); + return -ENOMEM; + } + ipc_mux->size_needed = le32_to_cpu(adb->adbh->block_length); + + ipc_mux->size_needed += offsetof(struct mux_adth, dg); + ipc_mux->size_needed += qlth_n_ql_size; + ipc_mux->size_needed += sizeof(*dg) + aligned_size; + return 0; +} + +/* Process encode session UL data. */ +static int mux_ul_dg_encode(struct iosm_mux *ipc_mux, struct mux_adb *adb, + struct mux_adth_dg *dg, + struct sk_buff_head *ul_list, + struct sk_buff *src_skb, int session_id, + int pkt_to_send, u32 qlth_n_ql_size, + int *out_offset, int head_pad_len) +{ + int aligned_size; + int offset = *out_offset; + unsigned long flags; + int nr_of_skb = 0; + + while (pkt_to_send > 0) { + /* Peek at the head of the list. */ + src_skb = skb_peek(ul_list); + if (!src_skb) { + dev_err(ipc_mux->dev, + "skb peek return NULL with count : %d", + pkt_to_send); + return -1; + } + aligned_size = ALIGN((head_pad_len + src_skb->len), 4); + ipc_mux->size_needed += sizeof(*dg) + aligned_size; + + if (ipc_mux->size_needed > adb->size || + ((ipc_mux->size_needed + ipc_mux->ul_data_pend_bytes) >= + IPC_MEM_MUX_UL_FLOWCTRL_HIGH_B)) { + *adb->next_table_index = offset; + if (mux_ul_dg_update_tbl_index(ipc_mux, session_id, + ul_list, dg, + aligned_size, + qlth_n_ql_size, adb, + src_skb) < 0) + return -ENOMEM; + nr_of_skb = 0; + offset = le32_to_cpu(adb->adbh->block_length); + /* Load pointer to next available datagram entry */ + dg = adb->dg[session_id] + adb->dg_count[session_id]; + } + /* Add buffer without head padding to next pending transfer. */ + memcpy(adb->buf + offset + head_pad_len, + src_skb->data, src_skb->len); + /* Setup datagram entry. */ + dg->datagram_index = cpu_to_le32(offset); + dg->datagram_length = cpu_to_le16(src_skb->len + head_pad_len); + dg->service_class = (((struct sk_buff *)src_skb)->priority); + dg->reserved = 0; + adb->dg_cnt_total++; + adb->payload_size += le16_to_cpu(dg->datagram_length); + dg++; + adb->dg_count[session_id]++; + offset += aligned_size; + /* Remove the processed elements and free it. */ + spin_lock_irqsave(&ul_list->lock, flags); + src_skb = __skb_dequeue(ul_list); + spin_unlock_irqrestore(&ul_list->lock, flags); + + dev_kfree_skb(src_skb); + nr_of_skb++; + pkt_to_send--; + } + *out_offset = offset; + return nr_of_skb; +} + +/* Process encode session UL data to ADB. */ +static int mux_ul_adb_encode(struct iosm_mux *ipc_mux, int session_id, + struct mux_session *session, + struct sk_buff_head *ul_list, struct mux_adb *adb, + int pkt_to_send) +{ + int adb_updated = -EINVAL; + int head_pad_len, offset; + struct sk_buff *src_skb = NULL; + struct mux_adth_dg *dg; + u32 qlth_n_ql_size; + + /* If any of the opened session has set Flow Control ON then limit the + * UL data to mux_flow_ctrl_high_thresh_b bytes + */ + if (ipc_mux->ul_data_pend_bytes >= + IPC_MEM_MUX_UL_FLOWCTRL_HIGH_B) { + ipc_mux_stop_tx_for_all_sessions(ipc_mux); + return adb_updated; + } + + qlth_n_ql_size = offsetof(struct mux_qlth, ql) + + MUX_QUEUE_LEVEL * sizeof(struct mux_qlth_ql); + head_pad_len = session->ul_head_pad_len; + + if (session->ul_head_pad_len > IPC_MEM_DL_ETH_OFFSET) + head_pad_len = session->ul_head_pad_len - IPC_MEM_DL_ETH_OFFSET; + + if (ipc_mux_ul_adb_allocate(ipc_mux, adb, &ipc_mux->size_needed, + IOSM_AGGR_MUX_SIG_ADBH)) + return -ENOMEM; + + offset = le32_to_cpu(adb->adbh->block_length); + + if (ipc_mux->size_needed == 0) + ipc_mux->size_needed = offset; + + /* Calculate the size needed for ADTH, QLTH and QL*/ + if (adb->dg_count[session_id] == 0) { + ipc_mux->size_needed += offsetof(struct mux_adth, dg); + ipc_mux->size_needed += qlth_n_ql_size; + } + + dg = adb->dg[session_id] + adb->dg_count[session_id]; + + if (mux_ul_dg_encode(ipc_mux, adb, dg, ul_list, src_skb, + session_id, pkt_to_send, qlth_n_ql_size, &offset, + head_pad_len) > 0) { + adb_updated = 1; + *adb->next_table_index = offset; + ipc_mux_ul_adb_update_ql(ipc_mux, adb, session_id, + qlth_n_ql_size, ul_list); + adb->adbh->block_length = cpu_to_le32(offset); + } + + return adb_updated; +} + bool ipc_mux_ul_data_encode(struct iosm_mux *ipc_mux) { struct sk_buff_head *ul_list; @@ -802,28 +1383,88 @@ bool ipc_mux_ul_data_encode(struct iosm_mux *ipc_mux) * -> try next session id. */ continue; - - updated = ipc_mux_ul_adgh_encode(ipc_mux, session_id, session, - ul_list, &ipc_mux->ul_adb, - dg_n); + if (ipc_mux->protocol == MUX_LITE) + updated = ipc_mux_ul_adgh_encode(ipc_mux, session_id, + session, ul_list, + &ipc_mux->ul_adb, + dg_n); + else + updated = mux_ul_adb_encode(ipc_mux, session_id, + session, ul_list, + &ipc_mux->ul_adb, + dg_n); } ipc_mux->adb_prep_ongoing = false; return updated == 1; } -void ipc_mux_ul_encoded_process(struct iosm_mux *ipc_mux, struct sk_buff *skb) +/* Calculates the Payload from any given ADB. */ +static int ipc_mux_get_payload_from_adb(struct iosm_mux *ipc_mux, + struct mux_adbh *p_adbh) { - struct mux_adgh *adgh; - u16 adgh_len; + struct mux_adth_dg *dg; + struct mux_adth *adth; + u32 payload_size = 0; + u32 next_table_idx; + int nr_of_dg, i; + + /* Process the aggregated datagram tables. */ + next_table_idx = le32_to_cpu(p_adbh->first_table_index); + + if (next_table_idx < sizeof(struct mux_adbh)) { + dev_err(ipc_mux->dev, "unexpected empty ADB"); + return payload_size; + } - adgh = (struct mux_adgh *)skb->data; - adgh_len = le16_to_cpu(adgh->length); + while (next_table_idx != 0) { + /* Get the reference to the table header. */ + adth = (struct mux_adth *)((u8 *)p_adbh + next_table_idx); - if (adgh->signature == cpu_to_le32(MUX_SIG_ADGH) && - ipc_mux->ul_flow == MUX_UL) - ipc_mux->ul_data_pend_bytes = ipc_mux->ul_data_pend_bytes - - adgh_len; + if (adth->signature == cpu_to_le32(IOSM_AGGR_MUX_SIG_ADTH)) { + nr_of_dg = (le16_to_cpu(adth->table_length) - + sizeof(struct mux_adth) + + sizeof(struct mux_adth_dg)) / + sizeof(struct mux_adth_dg); + + if (nr_of_dg <= 0) + return payload_size; + + dg = &adth->dg; + + for (i = 0; i < nr_of_dg; i++, dg++) { + if (le32_to_cpu(dg->datagram_index) < + sizeof(struct mux_adbh)) { + return payload_size; + } + payload_size += + le16_to_cpu(dg->datagram_length); + } + } + next_table_idx = le32_to_cpu(adth->next_table_index); + } + + return payload_size; +} + +void ipc_mux_ul_encoded_process(struct iosm_mux *ipc_mux, struct sk_buff *skb) +{ + union mux_type_header hr; + u16 adgh_len; + int payload; + + if (ipc_mux->protocol == MUX_LITE) { + hr.adgh = (struct mux_adgh *)skb->data; + adgh_len = le16_to_cpu(hr.adgh->length); + if (hr.adgh->signature == cpu_to_le32(IOSM_AGGR_MUX_SIG_ADGH) && + ipc_mux->ul_flow == MUX_UL) + ipc_mux->ul_data_pend_bytes = + ipc_mux->ul_data_pend_bytes - adgh_len; + } else { + hr.adbh = (struct mux_adbh *)(skb->data); + payload = ipc_mux_get_payload_from_adb(ipc_mux, hr.adbh); + ipc_mux->ul_data_pend_bytes -= payload; + } if (ipc_mux->ul_flow == MUX_UL) dev_dbg(ipc_mux->dev, "ul_data_pend_bytes: %lld", @@ -846,10 +1487,13 @@ static int ipc_mux_tq_ul_trigger_encode(struct iosm_imem *ipc_imem, int arg, /* Add session UL data to a ADB and ADGH */ ul_data_pend = ipc_mux_ul_data_encode(ipc_mux); - if (ul_data_pend) + if (ul_data_pend) { + if (ipc_mux->protocol == MUX_AGGREGATION) + ipc_imem_adb_timer_start(ipc_mux->imem); + /* Delay the doorbell irq */ ipc_imem_td_update_timer_start(ipc_mux->imem); - + } /* reset the debounce flag */ ipc_mux->ev_mux_net_transmit_pending = false; |