generic_bandwidth_allocation.c 9.5 KB

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