drivers/soundwire/generic_bandwidth_allocation.c

   1 // SPDX-License-Identifier: (GPL-2.0-only OR BSD-3-Clause)
   2 // Copyright(c) 2015-2020 Intel Corporation.
   3
   4 /*
   5  * Bandwidth management algorithm based on 2^n gears
   6  *
   7  */
   8
   9 #include <linux/device.h>
  10 #include <linux/module.h>
  11 #include <linux/mod_devicetable.h>
  12 #include <linux/slab.h>
  13 #include <linux/soundwire/sdw.h>
  14 #include "bus.h"
  15
  16 #define SDW_STRM_RATE_GROUPING          1
  17
  18 struct sdw_group_params {
  19         unsigned int rate;
  20         int full_bw;
  21         int payload_bw;
  22         int hwidth;
  23 };
  24
  25 struct sdw_group {
  26         unsigned int count;
  27         unsigned int max_size;
  28         unsigned int *rates;
  29 };
  30
  31 struct sdw_transport_data {
  32         int hstart;
  33         int hstop;
  34         int block_offset;
  35         int sub_block_offset;
  36 };
  37
  38 static void sdw_compute_slave_ports(struct sdw_master_runtime *m_rt,
  39                                     struct sdw_transport_data *t_data)
  40 {
  41         struct sdw_slave_runtime *s_rt = NULL;
  42         struct sdw_port_runtime *p_rt;
  43         int port_bo, sample_int;
  44         unsigned int rate, bps, ch = 0;
  45         unsigned int slave_total_ch;
  46         struct sdw_bus_params *b_params = &m_rt->bus->params;
  47
  48         port_bo = t_data->block_offset;
  49
  50         list_for_each_entry(s_rt, &m_rt->slave_rt_list, m_rt_node) {
  51                 rate = m_rt->stream->params.rate;
  52                 bps = m_rt->stream->params.bps;
  53                 sample_int = (m_rt->bus->params.curr_dr_freq / rate);
  54                 slave_total_ch = 0;
  55
  56                 list_for_each_entry(p_rt, &s_rt->port_list, port_node) {
  57                         ch = sdw_ch_mask_to_ch(p_rt->ch_mask);
  58
  59                         sdw_fill_xport_params(&p_rt->transport_params,
  60                                               p_rt->num, false,
  61                                               SDW_BLK_GRP_CNT_1,
  62                                               sample_int, port_bo, port_bo >> 8,
  63                                               t_data->hstart,
  64                                               t_data->hstop,
  65                                               (SDW_BLK_GRP_CNT_1 * ch), 0x0);
  66
  67                         sdw_fill_port_params(&p_rt->port_params,
  68                                              p_rt->num, bps,
  69                                              SDW_PORT_FLOW_MODE_ISOCH,
  70                                              b_params->s_data_mode);
  71
  72                         port_bo += bps * ch;
  73                         slave_total_ch += ch;
  74                 }
  75
  76                 if (m_rt->direction == SDW_DATA_DIR_TX &&
  77                     m_rt->ch_count == slave_total_ch) {
  78                         /*
  79                          * Slave devices were configured to access all channels
  80                          * of the stream, which indicates that they operate in
  81                          * 'mirror mode'. Make sure we reset the port offset for
  82                          * the next device in the list
  83                          */
  84                         port_bo = t_data->block_offset;
  85                 }
  86         }
  87 }
  88
  89 static void sdw_compute_master_ports(struct sdw_master_runtime *m_rt,
  90                                      struct sdw_group_params *params,
  91                                      int port_bo, int hstop)
  92 {
  93         struct sdw_transport_data t_data = {0};
  94         struct sdw_port_runtime *p_rt;
  95         struct sdw_bus *bus = m_rt->bus;
  96         struct sdw_bus_params *b_params = &bus->params;
  97         int sample_int, hstart = 0;
  98         unsigned int rate, bps, ch, no_ch;
  99
 100         rate = m_rt->stream->params.rate;
 101         bps = m_rt->stream->params.bps;
 102         ch = m_rt->ch_count;
 103         sample_int = (bus->params.curr_dr_freq / rate);
 104
 105         if (rate != params->rate)
 106                 return;
 107
 108         t_data.hstop = hstop;
 109         hstart = hstop - params->hwidth + 1;
 110         t_data.hstart = hstart;
 111
 112         list_for_each_entry(p_rt, &m_rt->port_list, port_node) {
 113                 no_ch = sdw_ch_mask_to_ch(p_rt->ch_mask);
 114
 115                 sdw_fill_xport_params(&p_rt->transport_params, p_rt->num,
 116                                       false, SDW_BLK_GRP_CNT_1, sample_int,
 117                                       port_bo, port_bo >> 8, hstart, hstop,
 118                                       (SDW_BLK_GRP_CNT_1 * no_ch), 0x0);
 119
 120                 sdw_fill_port_params(&p_rt->port_params,
 121                                      p_rt->num, bps,
 122                                      SDW_PORT_FLOW_MODE_ISOCH,
 123                                      b_params->m_data_mode);
 124
 125                 /* Check for first entry */
 126                 if (!(p_rt == list_first_entry(&m_rt->port_list,
 127                                                struct sdw_port_runtime,
 128                                                port_node))) {
 129                         port_bo += bps * ch;
 130                         continue;
 131                 }
 132
 133                 t_data.hstart = hstart;
 134                 t_data.hstop = hstop;
 135                 t_data.block_offset = port_bo;
 136                 t_data.sub_block_offset = 0;
 137                 port_bo += bps * ch;
 138         }
 139
 140         sdw_compute_slave_ports(m_rt, &t_data);
 141 }
 142
 143 static void _sdw_compute_port_params(struct sdw_bus *bus,
 144                                      struct sdw_group_params *params, int count)
 145 {
 146         struct sdw_master_runtime *m_rt = NULL;
 147         int hstop = bus->params.col - 1;
 148         int block_offset, port_bo, i;
 149
 150         /* Run loop for all groups to compute transport parameters */
 151         for (i = 0; i < count; i++) {
 152                 port_bo = 1;
 153                 block_offset = 1;
 154
 155                 list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
 156                         sdw_compute_master_ports(m_rt, &params[i],
 157                                                  port_bo, hstop);
 158
 159                         block_offset += m_rt->ch_count *
 160                                         m_rt->stream->params.bps;
 161                         port_bo = block_offset;
 162                 }
 163
 164                 hstop = hstop - params[i].hwidth;
 165         }
 166 }
 167
 168 static int sdw_compute_group_params(struct sdw_bus *bus,
 169                                     struct sdw_group_params *params,
 170                                     int *rates, int count)
 171 {
 172         struct sdw_master_runtime *m_rt = NULL;
 173         int sel_col = bus->params.col;
 174         unsigned int rate, bps, ch;
 175         int i, column_needed = 0;
 176
 177         /* Calculate bandwidth per group */
 178         for (i = 0; i < count; i++) {
 179                 params[i].rate = rates[i];
 180                 params[i].full_bw = bus->params.curr_dr_freq / params[i].rate;
 181         }
 182
 183         list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
 184                 rate = m_rt->stream->params.rate;
 185                 bps = m_rt->stream->params.bps;
 186                 ch = m_rt->ch_count;
 187
 188                 for (i = 0; i < count; i++) {
 189                         if (rate == params[i].rate)
 190                                 params[i].payload_bw += bps * ch;
 191                 }
 192         }
 193
 194         for (i = 0; i < count; i++) {
 195                 params[i].hwidth = (sel_col *
 196                         params[i].payload_bw + params[i].full_bw - 1) /
 197                         params[i].full_bw;
 198
 199                 column_needed += params[i].hwidth;
 200         }
 201
 202         if (column_needed > sel_col - 1)
 203                 return -EINVAL;
 204
 205         return 0;
 206 }
 207
 208 static int sdw_add_element_group_count(struct sdw_group *group,
 209                                        unsigned int rate)
 210 {
 211         int num = group->count;
 212         int i;
 213
 214         for (i = 0; i <= num; i++) {
 215                 if (rate == group->rates[i])
 216                         break;
 217
 218                 if (i != num)
 219                         continue;
 220
 221                 if (group->count >= group->max_size) {
 222                         unsigned int *rates;
 223
 224                         group->max_size += 1;
 225                         rates = krealloc(group->rates,
 226                                          (sizeof(int) * group->max_size),
 227                                          GFP_KERNEL);
 228                         if (!rates)
 229                                 return -ENOMEM;
 230                         group->rates = rates;
 231                 }
 232
 233                 group->rates[group->count++] = rate;
 234         }
 235
 236         return 0;
 237 }
 238
 239 static int sdw_get_group_count(struct sdw_bus *bus,
 240                                struct sdw_group *group)
 241 {
 242         struct sdw_master_runtime *m_rt;
 243         unsigned int rate;
 244         int ret = 0;
 245
 246         group->count = 0;
 247         group->max_size = SDW_STRM_RATE_GROUPING;
 248         group->rates = kcalloc(group->max_size, sizeof(int), GFP_KERNEL);
 249         if (!group->rates)
 250                 return -ENOMEM;
 251
 252         list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
 253                 rate = m_rt->stream->params.rate;
 254                 if (m_rt == list_first_entry(&bus->m_rt_list,
 255                                              struct sdw_master_runtime,
 256                                              bus_node)) {
 257                         group->rates[group->count++] = rate;
 258
 259                 } else {
 260                         ret = sdw_add_element_group_count(group, rate);
 261                         if (ret < 0) {
 262                                 kfree(group->rates);
 263                                 return ret;
 264                         }
 265                 }
 266         }
 267
 268         return ret;
 269 }
 270
 271 /**
 272  * sdw_compute_port_params: Compute transport and port parameters
 273  *
 274  * @bus: SDW Bus instance
 275  */
 276 static int sdw_compute_port_params(struct sdw_bus *bus)
 277 {
 278         struct sdw_group_params *params = NULL;
 279         struct sdw_group group;
 280         int ret;
 281
 282         ret = sdw_get_group_count(bus, &group);
 283         if (ret < 0)
 284                 return ret;
 285
 286         if (group.count == 0)
 287                 goto out;
 288
 289         params = kcalloc(group.count, sizeof(*params), GFP_KERNEL);
 290         if (!params) {
 291                 ret = -ENOMEM;
 292                 goto out;
 293         }
 294
 295         /* Compute transport parameters for grouped streams */
 296         ret = sdw_compute_group_params(bus, params,
 297                                        &group.rates[0], group.count);
 298         if (ret < 0)
 299                 goto free_params;
 300
 301         _sdw_compute_port_params(bus, params, group.count);
 302
 303 free_params:
 304         kfree(params);
 305 out:
 306         kfree(group.rates);
 307
 308         return ret;
 309 }
 310
 311 static int sdw_select_row_col(struct sdw_bus *bus, int clk_freq)
 312 {
 313         struct sdw_master_prop *prop = &bus->prop;
 314         int frame_int, frame_freq;
 315         int r, c;
 316
 317         for (c = 0; c < SDW_FRAME_COLS; c++) {
 318                 for (r = 0; r < SDW_FRAME_ROWS; r++) {
 319                         if (sdw_rows[r] != prop->default_row ||
 320                             sdw_cols[c] != prop->default_col)
 321                                 continue;
 322
 323                         frame_int = sdw_rows[r] * sdw_cols[c];
 324                         frame_freq = clk_freq / frame_int;
 325
 326                         if ((clk_freq - (frame_freq * SDW_FRAME_CTRL_BITS)) <
 327                             bus->params.bandwidth)
 328                                 continue;
 329
 330                         bus->params.row = sdw_rows[r];
 331                         bus->params.col = sdw_cols[c];
 332                         return 0;
 333                 }
 334         }
 335
 336         return -EINVAL;
 337 }
 338
 339 /**
 340  * sdw_compute_bus_params: Compute bus parameters
 341  *
 342  * @bus: SDW Bus instance
 343  */
 344 static int sdw_compute_bus_params(struct sdw_bus *bus)
 345 {
 346         unsigned int max_dr_freq, curr_dr_freq = 0;
 347         struct sdw_master_prop *mstr_prop = &bus->prop;
 348         int i, clk_values, ret;
 349         bool is_gear = false;
 350         u32 *clk_buf;
 351
 352         if (mstr_prop->num_clk_gears) {
 353                 clk_values = mstr_prop->num_clk_gears;
 354                 clk_buf = mstr_prop->clk_gears;
 355                 is_gear = true;
 356         } else if (mstr_prop->num_clk_freq) {
 357                 clk_values = mstr_prop->num_clk_freq;
 358                 clk_buf = mstr_prop->clk_freq;
 359         } else {
 360                 clk_values = 1;
 361                 clk_buf = NULL;
 362         }
 363
 364         max_dr_freq = mstr_prop->max_clk_freq * SDW_DOUBLE_RATE_FACTOR;
 365
 366         for (i = 0; i < clk_values; i++) {
 367                 if (!clk_buf)
 368                         curr_dr_freq = max_dr_freq;
 369                 else
 370                         curr_dr_freq = (is_gear) ?
 371                                 (max_dr_freq >>  clk_buf[i]) :
 372                                 clk_buf[i] * SDW_DOUBLE_RATE_FACTOR;
 373
 374                 if (curr_dr_freq <= bus->params.bandwidth)
 375                         continue;
 376
 377                 break;
 378
 379                 /*
 380                  * TODO: Check all the Slave(s) port(s) audio modes and find
 381                  * whether given clock rate is supported with glitchless
 382                  * transition.
 383                  */
 384         }
 385
 386         if (i == clk_values)
 387                 return -EINVAL;
 388
 389         ret = sdw_select_row_col(bus, curr_dr_freq);
 390         if (ret < 0)
 391                 return -EINVAL;
 392
 393         bus->params.curr_dr_freq = curr_dr_freq;
 394         return 0;
 395 }
 396
 397 /**
 398  * sdw_compute_params: Compute bus, transport and port parameters
 399  *
 400  * @bus: SDW Bus instance
 401  */
 402 int sdw_compute_params(struct sdw_bus *bus)
 403 {
 404         int ret;
 405
 406         /* Computes clock frequency, frame shape and frame frequency */
 407         ret = sdw_compute_bus_params(bus);
 408         if (ret < 0) {
 409                 dev_err(bus->dev, "Compute bus params failed: %d", ret);
 410                 return ret;
 411         }
 412
 413         /* Compute transport and port params */
 414         ret = sdw_compute_port_params(bus);
 415         if (ret < 0) {
 416                 dev_err(bus->dev, "Compute transport params failed: %d", ret);
 417                 return ret;
 418         }
 419
 420         return 0;
 421 }
 422 EXPORT_SYMBOL(sdw_compute_params);
 423
 424 MODULE_LICENSE("Dual BSD/GPL");
 425 MODULE_DESCRIPTION("SoundWire Generic Bandwidth Allocation");