linux/drivers/soundwire/generic_bandwidth_allocation.c
<<
>>
Prefs
   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
  18struct sdw_group_params {
  19        unsigned int rate;
  20        int full_bw;
  21        int payload_bw;
  22        int hwidth;
  23};
  24
  25struct sdw_group {
  26        unsigned int count;
  27        unsigned int max_size;
  28        unsigned int *rates;
  29};
  30
  31struct sdw_transport_data {
  32        int hstart;
  33        int hstop;
  34        int block_offset;
  35        int sub_block_offset;
  36};
  37
  38static 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_PKG_PER_PORT, 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
  89static 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;
  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
 114                sdw_fill_xport_params(&p_rt->transport_params, p_rt->num,
 115                                      false, SDW_BLK_GRP_CNT_1, sample_int,
 116                                      port_bo, port_bo >> 8, hstart, hstop,
 117                                      SDW_BLK_PKG_PER_PORT, 0x0);
 118
 119                sdw_fill_port_params(&p_rt->port_params,
 120                                     p_rt->num, bps,
 121                                     SDW_PORT_FLOW_MODE_ISOCH,
 122                                     b_params->m_data_mode);
 123
 124                /* Check for first entry */
 125                if (!(p_rt == list_first_entry(&m_rt->port_list,
 126                                               struct sdw_port_runtime,
 127                                               port_node))) {
 128                        port_bo += bps * ch;
 129                        continue;
 130                }
 131
 132                t_data.hstart = hstart;
 133                t_data.hstop = hstop;
 134                t_data.block_offset = port_bo;
 135                t_data.sub_block_offset = 0;
 136                port_bo += bps * ch;
 137        }
 138
 139        sdw_compute_slave_ports(m_rt, &t_data);
 140}
 141
 142static void _sdw_compute_port_params(struct sdw_bus *bus,
 143                                     struct sdw_group_params *params, int count)
 144{
 145        struct sdw_master_runtime *m_rt;
 146        int hstop = bus->params.col - 1;
 147        int block_offset, port_bo, i;
 148
 149        /* Run loop for all groups to compute transport parameters */
 150        for (i = 0; i < count; i++) {
 151                port_bo = 1;
 152                block_offset = 1;
 153
 154                list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
 155                        sdw_compute_master_ports(m_rt, &params[i],
 156                                                 port_bo, hstop);
 157
 158                        block_offset += m_rt->ch_count *
 159                                        m_rt->stream->params.bps;
 160                        port_bo = block_offset;
 161                }
 162
 163                hstop = hstop - params[i].hwidth;
 164        }
 165}
 166
 167static int sdw_compute_group_params(struct sdw_bus *bus,
 168                                    struct sdw_group_params *params,
 169                                    int *rates, int count)
 170{
 171        struct sdw_master_runtime *m_rt;
 172        int sel_col = bus->params.col;
 173        unsigned int rate, bps, ch;
 174        int i, column_needed = 0;
 175
 176        /* Calculate bandwidth per group */
 177        for (i = 0; i < count; i++) {
 178                params[i].rate = rates[i];
 179                params[i].full_bw = bus->params.curr_dr_freq / params[i].rate;
 180        }
 181
 182        list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
 183                rate = m_rt->stream->params.rate;
 184                bps = m_rt->stream->params.bps;
 185                ch = m_rt->ch_count;
 186
 187                for (i = 0; i < count; i++) {
 188                        if (rate == params[i].rate)
 189                                params[i].payload_bw += bps * ch;
 190                }
 191        }
 192
 193        for (i = 0; i < count; i++) {
 194                params[i].hwidth = (sel_col *
 195                        params[i].payload_bw + params[i].full_bw - 1) /
 196                        params[i].full_bw;
 197
 198                column_needed += params[i].hwidth;
 199        }
 200
 201        if (column_needed > sel_col - 1)
 202                return -EINVAL;
 203
 204        return 0;
 205}
 206
 207static int sdw_add_element_group_count(struct sdw_group *group,
 208                                       unsigned int rate)
 209{
 210        int num = group->count;
 211        int i;
 212
 213        for (i = 0; i <= num; i++) {
 214                if (rate == group->rates[i])
 215                        break;
 216
 217                if (i != num)
 218                        continue;
 219
 220                if (group->count >= group->max_size) {
 221                        unsigned int *rates;
 222
 223                        group->max_size += 1;
 224                        rates = krealloc(group->rates,
 225                                         (sizeof(int) * group->max_size),
 226                                         GFP_KERNEL);
 227                        if (!rates)
 228                                return -ENOMEM;
 229                        group->rates = rates;
 230                }
 231
 232                group->rates[group->count++] = rate;
 233        }
 234
 235        return 0;
 236}
 237
 238static int sdw_get_group_count(struct sdw_bus *bus,
 239                               struct sdw_group *group)
 240{
 241        struct sdw_master_runtime *m_rt;
 242        unsigned int rate;
 243        int ret = 0;
 244
 245        group->count = 0;
 246        group->max_size = SDW_STRM_RATE_GROUPING;
 247        group->rates = kcalloc(group->max_size, sizeof(int), GFP_KERNEL);
 248        if (!group->rates)
 249                return -ENOMEM;
 250
 251        list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
 252                rate = m_rt->stream->params.rate;
 253                if (m_rt == list_first_entry(&bus->m_rt_list,
 254                                             struct sdw_master_runtime,
 255                                             bus_node)) {
 256                        group->rates[group->count++] = rate;
 257
 258                } else {
 259                        ret = sdw_add_element_group_count(group, rate);
 260                        if (ret < 0) {
 261                                kfree(group->rates);
 262                                return ret;
 263                        }
 264                }
 265        }
 266
 267        return ret;
 268}
 269
 270/**
 271 * sdw_compute_port_params: Compute transport and port parameters
 272 *
 273 * @bus: SDW Bus instance
 274 */
 275static int sdw_compute_port_params(struct sdw_bus *bus)
 276{
 277        struct sdw_group_params *params = NULL;
 278        struct sdw_group group;
 279        int ret;
 280
 281        ret = sdw_get_group_count(bus, &group);
 282        if (ret < 0)
 283                return ret;
 284
 285        if (group.count == 0)
 286                goto out;
 287
 288        params = kcalloc(group.count, sizeof(*params), GFP_KERNEL);
 289        if (!params) {
 290                ret = -ENOMEM;
 291                goto out;
 292        }
 293
 294        /* Compute transport parameters for grouped streams */
 295        ret = sdw_compute_group_params(bus, params,
 296                                       &group.rates[0], group.count);
 297        if (ret < 0)
 298                goto free_params;
 299
 300        _sdw_compute_port_params(bus, params, group.count);
 301
 302free_params:
 303        kfree(params);
 304out:
 305        kfree(group.rates);
 306
 307        return ret;
 308}
 309
 310static int sdw_select_row_col(struct sdw_bus *bus, int clk_freq)
 311{
 312        struct sdw_master_prop *prop = &bus->prop;
 313        int frame_int, frame_freq;
 314        int r, c;
 315
 316        for (c = 0; c < SDW_FRAME_COLS; c++) {
 317                for (r = 0; r < SDW_FRAME_ROWS; r++) {
 318                        if (sdw_rows[r] != prop->default_row ||
 319                            sdw_cols[c] != prop->default_col)
 320                                continue;
 321
 322                        frame_int = sdw_rows[r] * sdw_cols[c];
 323                        frame_freq = clk_freq / frame_int;
 324
 325                        if ((clk_freq - (frame_freq * SDW_FRAME_CTRL_BITS)) <
 326                            bus->params.bandwidth)
 327                                continue;
 328
 329                        bus->params.row = sdw_rows[r];
 330                        bus->params.col = sdw_cols[c];
 331                        return 0;
 332                }
 333        }
 334
 335        return -EINVAL;
 336}
 337
 338/**
 339 * sdw_compute_bus_params: Compute bus parameters
 340 *
 341 * @bus: SDW Bus instance
 342 */
 343static int sdw_compute_bus_params(struct sdw_bus *bus)
 344{
 345        unsigned int max_dr_freq, curr_dr_freq = 0;
 346        struct sdw_master_prop *mstr_prop = &bus->prop;
 347        int i, clk_values, ret;
 348        bool is_gear = false;
 349        u32 *clk_buf;
 350
 351        if (mstr_prop->num_clk_gears) {
 352                clk_values = mstr_prop->num_clk_gears;
 353                clk_buf = mstr_prop->clk_gears;
 354                is_gear = true;
 355        } else if (mstr_prop->num_clk_freq) {
 356                clk_values = mstr_prop->num_clk_freq;
 357                clk_buf = mstr_prop->clk_freq;
 358        } else {
 359                clk_values = 1;
 360                clk_buf = NULL;
 361        }
 362
 363        max_dr_freq = mstr_prop->max_clk_freq * SDW_DOUBLE_RATE_FACTOR;
 364
 365        for (i = 0; i < clk_values; i++) {
 366                if (!clk_buf)
 367                        curr_dr_freq = max_dr_freq;
 368                else
 369                        curr_dr_freq = (is_gear) ?
 370                                (max_dr_freq >>  clk_buf[i]) :
 371                                clk_buf[i] * SDW_DOUBLE_RATE_FACTOR;
 372
 373                if (curr_dr_freq <= bus->params.bandwidth)
 374                        continue;
 375
 376                break;
 377
 378                /*
 379                 * TODO: Check all the Slave(s) port(s) audio modes and find
 380                 * whether given clock rate is supported with glitchless
 381                 * transition.
 382                 */
 383        }
 384
 385        if (i == clk_values)
 386                return -EINVAL;
 387
 388        ret = sdw_select_row_col(bus, curr_dr_freq);
 389        if (ret < 0)
 390                return -EINVAL;
 391
 392        bus->params.curr_dr_freq = curr_dr_freq;
 393        return 0;
 394}
 395
 396/**
 397 * sdw_compute_params: Compute bus, transport and port parameters
 398 *
 399 * @bus: SDW Bus instance
 400 */
 401int sdw_compute_params(struct sdw_bus *bus)
 402{
 403        int ret;
 404
 405        /* Computes clock frequency, frame shape and frame frequency */
 406        ret = sdw_compute_bus_params(bus);
 407        if (ret < 0) {
 408                dev_err(bus->dev, "Compute bus params failed: %d\n", ret);
 409                return ret;
 410        }
 411
 412        /* Compute transport and port params */
 413        ret = sdw_compute_port_params(bus);
 414        if (ret < 0) {
 415                dev_err(bus->dev, "Compute transport params failed: %d\n", ret);
 416                return ret;
 417        }
 418
 419        return 0;
 420}
 421EXPORT_SYMBOL(sdw_compute_params);
 422
 423MODULE_LICENSE("Dual BSD/GPL");
 424MODULE_DESCRIPTION("SoundWire Generic Bandwidth Allocation");
 425