drivers/soundwire/generic_bandwidth_allocation.c

0001 // SPDX-License-Identifier: (GPL-2.0-only OR BSD-3-Clause)
0002 // Copyright(c) 2015-2020 Intel Corporation.
0003
0004 /*
0005  * Bandwidth management algorithm based on 2^n gears
0006  *
0007  */
0008
0009 #include <linux/device.h>
0010 #include <linux/module.h>
0011 #include <linux/mod_devicetable.h>
0012 #include <linux/slab.h>
0013 #include <linux/soundwire/sdw.h>
0014 #include "bus.h"
0015
0016 #define SDW_STRM_RATE_GROUPING      1
0017
0018 struct sdw_group_params {
0019     unsigned int rate;
0020     int full_bw;
0021     int payload_bw;
0022     int hwidth;
0023 };
0024
0025 struct sdw_group {
0026     unsigned int count;
0027     unsigned int max_size;
0028     unsigned int *rates;
0029 };
0030
0031 struct sdw_transport_data {
0032     int hstart;
0033     int hstop;
0034     int block_offset;
0035     int sub_block_offset;
0036 };
0037
0038 static void sdw_compute_slave_ports(struct sdw_master_runtime *m_rt,
0039                     struct sdw_transport_data *t_data)
0040 {
0041     struct sdw_slave_runtime *s_rt = NULL;
0042     struct sdw_port_runtime *p_rt;
0043     int port_bo, sample_int;
0044     unsigned int rate, bps, ch = 0;
0045     unsigned int slave_total_ch;
0046     struct sdw_bus_params *b_params = &m_rt->bus->params;
0047
0048     port_bo = t_data->block_offset;
0049
0050     list_for_each_entry(s_rt, &m_rt->slave_rt_list, m_rt_node) {
0051         rate = m_rt->stream->params.rate;
0052         bps = m_rt->stream->params.bps;
0053         sample_int = (m_rt->bus->params.curr_dr_freq / rate);
0054         slave_total_ch = 0;
0055
0056         list_for_each_entry(p_rt, &s_rt->port_list, port_node) {
0057             ch = sdw_ch_mask_to_ch(p_rt->ch_mask);
0058
0059             sdw_fill_xport_params(&p_rt->transport_params,
0060                           p_rt->num, false,
0061                           SDW_BLK_GRP_CNT_1,
0062                           sample_int, port_bo, port_bo >> 8,
0063                           t_data->hstart,
0064                           t_data->hstop,
0065                           SDW_BLK_PKG_PER_PORT, 0x0);
0066
0067             sdw_fill_port_params(&p_rt->port_params,
0068                          p_rt->num, bps,
0069                          SDW_PORT_FLOW_MODE_ISOCH,
0070                          b_params->s_data_mode);
0071
0072             port_bo += bps * ch;
0073             slave_total_ch += ch;
0074         }
0075
0076         if (m_rt->direction == SDW_DATA_DIR_TX &&
0077             m_rt->ch_count == slave_total_ch) {
0078             /*
0079              * Slave devices were configured to access all channels
0080              * of the stream, which indicates that they operate in
0081              * 'mirror mode'. Make sure we reset the port offset for
0082              * the next device in the list
0083              */
0084             port_bo = t_data->block_offset;
0085         }
0086     }
0087 }
0088
0089 static void sdw_compute_master_ports(struct sdw_master_runtime *m_rt,
0090                      struct sdw_group_params *params,
0091                      int port_bo, int hstop)
0092 {
0093     struct sdw_transport_data t_data = {0};
0094     struct sdw_port_runtime *p_rt;
0095     struct sdw_bus *bus = m_rt->bus;
0096     struct sdw_bus_params *b_params = &bus->params;
0097     int sample_int, hstart = 0;
0098     unsigned int rate, bps, ch;
0099
0100     rate = m_rt->stream->params.rate;
0101     bps = m_rt->stream->params.bps;
0102     ch = m_rt->ch_count;
0103     sample_int = (bus->params.curr_dr_freq / rate);
0104
0105     if (rate != params->rate)
0106         return;
0107
0108     t_data.hstop = hstop;
0109     hstart = hstop - params->hwidth + 1;
0110     t_data.hstart = hstart;
0111
0112     list_for_each_entry(p_rt, &m_rt->port_list, port_node) {
0113
0114         sdw_fill_xport_params(&p_rt->transport_params, p_rt->num,
0115                       false, SDW_BLK_GRP_CNT_1, sample_int,
0116                       port_bo, port_bo >> 8, hstart, hstop,
0117                       SDW_BLK_PKG_PER_PORT, 0x0);
0118
0119         sdw_fill_port_params(&p_rt->port_params,
0120                      p_rt->num, bps,
0121                      SDW_PORT_FLOW_MODE_ISOCH,
0122                      b_params->m_data_mode);
0123
0124         /* Check for first entry */
0125         if (!(p_rt == list_first_entry(&m_rt->port_list,
0126                            struct sdw_port_runtime,
0127                            port_node))) {
0128             port_bo += bps * ch;
0129             continue;
0130         }
0131
0132         t_data.hstart = hstart;
0133         t_data.hstop = hstop;
0134         t_data.block_offset = port_bo;
0135         t_data.sub_block_offset = 0;
0136         port_bo += bps * ch;
0137     }
0138
0139     sdw_compute_slave_ports(m_rt, &t_data);
0140 }
0141
0142 static void _sdw_compute_port_params(struct sdw_bus *bus,
0143                      struct sdw_group_params *params, int count)
0144 {
0145     struct sdw_master_runtime *m_rt;
0146     int hstop = bus->params.col - 1;
0147     int block_offset, port_bo, i;
0148
0149     /* Run loop for all groups to compute transport parameters */
0150     for (i = 0; i < count; i++) {
0151         port_bo = 1;
0152         block_offset = 1;
0153
0154         list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
0155             sdw_compute_master_ports(m_rt, &params[i],
0156                          port_bo, hstop);
0157
0158             block_offset += m_rt->ch_count *
0159                     m_rt->stream->params.bps;
0160             port_bo = block_offset;
0161         }
0162
0163         hstop = hstop - params[i].hwidth;
0164     }
0165 }
0166
0167 static int sdw_compute_group_params(struct sdw_bus *bus,
0168                     struct sdw_group_params *params,
0169                     int *rates, int count)
0170 {
0171     struct sdw_master_runtime *m_rt;
0172     int sel_col = bus->params.col;
0173     unsigned int rate, bps, ch;
0174     int i, column_needed = 0;
0175
0176     /* Calculate bandwidth per group */
0177     for (i = 0; i < count; i++) {
0178         params[i].rate = rates[i];
0179         params[i].full_bw = bus->params.curr_dr_freq / params[i].rate;
0180     }
0181
0182     list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
0183         rate = m_rt->stream->params.rate;
0184         bps = m_rt->stream->params.bps;
0185         ch = m_rt->ch_count;
0186
0187         for (i = 0; i < count; i++) {
0188             if (rate == params[i].rate)
0189                 params[i].payload_bw += bps * ch;
0190         }
0191     }
0192
0193     for (i = 0; i < count; i++) {
0194         params[i].hwidth = (sel_col *
0195             params[i].payload_bw + params[i].full_bw - 1) /
0196             params[i].full_bw;
0197
0198         column_needed += params[i].hwidth;
0199     }
0200
0201     if (column_needed > sel_col - 1)
0202         return -EINVAL;
0203
0204     return 0;
0205 }
0206
0207 static int sdw_add_element_group_count(struct sdw_group *group,
0208                        unsigned int rate)
0209 {
0210     int num = group->count;
0211     int i;
0212
0213     for (i = 0; i <= num; i++) {
0214         if (rate == group->rates[i])
0215             break;
0216
0217         if (i != num)
0218             continue;
0219
0220         if (group->count >= group->max_size) {
0221             unsigned int *rates;
0222
0223             group->max_size += 1;
0224             rates = krealloc(group->rates,
0225                      (sizeof(int) * group->max_size),
0226                      GFP_KERNEL);
0227             if (!rates)
0228                 return -ENOMEM;
0229             group->rates = rates;
0230         }
0231
0232         group->rates[group->count++] = rate;
0233     }
0234
0235     return 0;
0236 }
0237
0238 static int sdw_get_group_count(struct sdw_bus *bus,
0239                    struct sdw_group *group)
0240 {
0241     struct sdw_master_runtime *m_rt;
0242     unsigned int rate;
0243     int ret = 0;
0244
0245     group->count = 0;
0246     group->max_size = SDW_STRM_RATE_GROUPING;
0247     group->rates = kcalloc(group->max_size, sizeof(int), GFP_KERNEL);
0248     if (!group->rates)
0249         return -ENOMEM;
0250
0251     list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
0252         rate = m_rt->stream->params.rate;
0253         if (m_rt == list_first_entry(&bus->m_rt_list,
0254                          struct sdw_master_runtime,
0255                          bus_node)) {
0256             group->rates[group->count++] = rate;
0257
0258         } else {
0259             ret = sdw_add_element_group_count(group, rate);
0260             if (ret < 0) {
0261                 kfree(group->rates);
0262                 return ret;
0263             }
0264         }
0265     }
0266
0267     return ret;
0268 }
0269
0270 /**
0271  * sdw_compute_port_params: Compute transport and port parameters
0272  *
0273  * @bus: SDW Bus instance
0274  */
0275 static int sdw_compute_port_params(struct sdw_bus *bus)
0276 {
0277     struct sdw_group_params *params = NULL;
0278     struct sdw_group group;
0279     int ret;
0280
0281     ret = sdw_get_group_count(bus, &group);
0282     if (ret < 0)
0283         return ret;
0284
0285     if (group.count == 0)
0286         goto out;
0287
0288     params = kcalloc(group.count, sizeof(*params), GFP_KERNEL);
0289     if (!params) {
0290         ret = -ENOMEM;
0291         goto out;
0292     }
0293
0294     /* Compute transport parameters for grouped streams */
0295     ret = sdw_compute_group_params(bus, params,
0296                        &group.rates[0], group.count);
0297     if (ret < 0)
0298         goto free_params;
0299
0300     _sdw_compute_port_params(bus, params, group.count);
0301
0302 free_params:
0303     kfree(params);
0304 out:
0305     kfree(group.rates);
0306
0307     return ret;
0308 }
0309
0310 static int sdw_select_row_col(struct sdw_bus *bus, int clk_freq)
0311 {
0312     struct sdw_master_prop *prop = &bus->prop;
0313     int frame_int, frame_freq;
0314     int r, c;
0315
0316     for (c = 0; c < SDW_FRAME_COLS; c++) {
0317         for (r = 0; r < SDW_FRAME_ROWS; r++) {
0318             if (sdw_rows[r] != prop->default_row ||
0319                 sdw_cols[c] != prop->default_col)
0320                 continue;
0321
0322             frame_int = sdw_rows[r] * sdw_cols[c];
0323             frame_freq = clk_freq / frame_int;
0324
0325             if ((clk_freq - (frame_freq * SDW_FRAME_CTRL_BITS)) <
0326                 bus->params.bandwidth)
0327                 continue;
0328
0329             bus->params.row = sdw_rows[r];
0330             bus->params.col = sdw_cols[c];
0331             return 0;
0332         }
0333     }
0334
0335     return -EINVAL;
0336 }
0337
0338 /**
0339  * sdw_compute_bus_params: Compute bus parameters
0340  *
0341  * @bus: SDW Bus instance
0342  */
0343 static int sdw_compute_bus_params(struct sdw_bus *bus)
0344 {
0345     unsigned int max_dr_freq, curr_dr_freq = 0;
0346     struct sdw_master_prop *mstr_prop = &bus->prop;
0347     int i, clk_values, ret;
0348     bool is_gear = false;
0349     u32 *clk_buf;
0350
0351     if (mstr_prop->num_clk_gears) {
0352         clk_values = mstr_prop->num_clk_gears;
0353         clk_buf = mstr_prop->clk_gears;
0354         is_gear = true;
0355     } else if (mstr_prop->num_clk_freq) {
0356         clk_values = mstr_prop->num_clk_freq;
0357         clk_buf = mstr_prop->clk_freq;
0358     } else {
0359         clk_values = 1;
0360         clk_buf = NULL;
0361     }
0362
0363     max_dr_freq = mstr_prop->max_clk_freq * SDW_DOUBLE_RATE_FACTOR;
0364
0365     for (i = 0; i < clk_values; i++) {
0366         if (!clk_buf)
0367             curr_dr_freq = max_dr_freq;
0368         else
0369             curr_dr_freq = (is_gear) ?
0370                 (max_dr_freq >>  clk_buf[i]) :
0371                 clk_buf[i] * SDW_DOUBLE_RATE_FACTOR;
0372
0373         if (curr_dr_freq <= bus->params.bandwidth)
0374             continue;
0375
0376         break;
0377
0378         /*
0379          * TODO: Check all the Slave(s) port(s) audio modes and find
0380          * whether given clock rate is supported with glitchless
0381          * transition.
0382          */
0383     }
0384
0385     if (i == clk_values) {
0386         dev_err(bus->dev, "%s: could not find clock value for bandwidth %d\n",
0387             __func__, bus->params.bandwidth);
0388         return -EINVAL;
0389     }
0390
0391     ret = sdw_select_row_col(bus, curr_dr_freq);
0392     if (ret < 0) {
0393         dev_err(bus->dev, "%s: could not find frame configuration for bus dr_freq %d\n",
0394             __func__, curr_dr_freq);
0395         return -EINVAL;
0396     }
0397
0398     bus->params.curr_dr_freq = curr_dr_freq;
0399     return 0;
0400 }
0401
0402 /**
0403  * sdw_compute_params: Compute bus, transport and port parameters
0404  *
0405  * @bus: SDW Bus instance
0406  */
0407 int sdw_compute_params(struct sdw_bus *bus)
0408 {
0409     int ret;
0410
0411     /* Computes clock frequency, frame shape and frame frequency */
0412     ret = sdw_compute_bus_params(bus);
0413     if (ret < 0)
0414         return ret;
0415
0416     /* Compute transport and port params */
0417     ret = sdw_compute_port_params(bus);
0418     if (ret < 0) {
0419         dev_err(bus->dev, "Compute transport params failed: %d\n", ret);
0420         return ret;
0421     }
0422
0423     return 0;
0424 }
0425 EXPORT_SYMBOL(sdw_compute_params);
0426
0427 MODULE_LICENSE("Dual BSD/GPL");
0428 MODULE_DESCRIPTION("SoundWire Generic Bandwidth Allocation");