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