clock.c 8.4 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377
  1. // SPDX-License-Identifier: GPL-2.0
  2. /*
  3. * System Control and Management Interface (SCMI) Clock Protocol
  4. *
  5. * Copyright (C) 2018-2020 ARM Ltd.
  6. */
  7. #include <linux/module.h>
  8. #include <linux/sort.h>
  9. #include "common.h"
  10. enum scmi_clock_protocol_cmd {
  11. CLOCK_ATTRIBUTES = 0x3,
  12. CLOCK_DESCRIBE_RATES = 0x4,
  13. CLOCK_RATE_SET = 0x5,
  14. CLOCK_RATE_GET = 0x6,
  15. CLOCK_CONFIG_SET = 0x7,
  16. };
  17. struct scmi_msg_resp_clock_protocol_attributes {
  18. __le16 num_clocks;
  19. u8 max_async_req;
  20. u8 reserved;
  21. };
  22. struct scmi_msg_resp_clock_attributes {
  23. __le32 attributes;
  24. #define CLOCK_ENABLE BIT(0)
  25. u8 name[SCMI_MAX_STR_SIZE];
  26. };
  27. struct scmi_clock_set_config {
  28. __le32 id;
  29. __le32 attributes;
  30. };
  31. struct scmi_msg_clock_describe_rates {
  32. __le32 id;
  33. __le32 rate_index;
  34. };
  35. struct scmi_msg_resp_clock_describe_rates {
  36. __le32 num_rates_flags;
  37. #define NUM_RETURNED(x) ((x) & 0xfff)
  38. #define RATE_DISCRETE(x) !((x) & BIT(12))
  39. #define NUM_REMAINING(x) ((x) >> 16)
  40. struct {
  41. __le32 value_low;
  42. __le32 value_high;
  43. } rate[0];
  44. #define RATE_TO_U64(X) \
  45. ({ \
  46. typeof(X) x = (X); \
  47. le32_to_cpu((x).value_low) | (u64)le32_to_cpu((x).value_high) << 32; \
  48. })
  49. };
  50. struct scmi_clock_set_rate {
  51. __le32 flags;
  52. #define CLOCK_SET_ASYNC BIT(0)
  53. #define CLOCK_SET_IGNORE_RESP BIT(1)
  54. #define CLOCK_SET_ROUND_UP BIT(2)
  55. #define CLOCK_SET_ROUND_AUTO BIT(3)
  56. __le32 id;
  57. __le32 value_low;
  58. __le32 value_high;
  59. };
  60. struct clock_info {
  61. u32 version;
  62. int num_clocks;
  63. int max_async_req;
  64. atomic_t cur_async_req;
  65. struct scmi_clock_info *clk;
  66. };
  67. static int
  68. scmi_clock_protocol_attributes_get(const struct scmi_protocol_handle *ph,
  69. struct clock_info *ci)
  70. {
  71. int ret;
  72. struct scmi_xfer *t;
  73. struct scmi_msg_resp_clock_protocol_attributes *attr;
  74. ret = ph->xops->xfer_get_init(ph, PROTOCOL_ATTRIBUTES,
  75. 0, sizeof(*attr), &t);
  76. if (ret)
  77. return ret;
  78. attr = t->rx.buf;
  79. ret = ph->xops->do_xfer(ph, t);
  80. if (!ret) {
  81. ci->num_clocks = le16_to_cpu(attr->num_clocks);
  82. ci->max_async_req = attr->max_async_req;
  83. }
  84. ph->xops->xfer_put(ph, t);
  85. return ret;
  86. }
  87. static int scmi_clock_attributes_get(const struct scmi_protocol_handle *ph,
  88. u32 clk_id, struct scmi_clock_info *clk)
  89. {
  90. int ret;
  91. struct scmi_xfer *t;
  92. struct scmi_msg_resp_clock_attributes *attr;
  93. ret = ph->xops->xfer_get_init(ph, CLOCK_ATTRIBUTES,
  94. sizeof(clk_id), sizeof(*attr), &t);
  95. if (ret)
  96. return ret;
  97. put_unaligned_le32(clk_id, t->tx.buf);
  98. attr = t->rx.buf;
  99. ret = ph->xops->do_xfer(ph, t);
  100. if (!ret)
  101. strlcpy(clk->name, attr->name, SCMI_MAX_STR_SIZE);
  102. else
  103. clk->name[0] = '\0';
  104. ph->xops->xfer_put(ph, t);
  105. return ret;
  106. }
  107. static int rate_cmp_func(const void *_r1, const void *_r2)
  108. {
  109. const u64 *r1 = _r1, *r2 = _r2;
  110. if (*r1 < *r2)
  111. return -1;
  112. else if (*r1 == *r2)
  113. return 0;
  114. else
  115. return 1;
  116. }
  117. static int
  118. scmi_clock_describe_rates_get(const struct scmi_protocol_handle *ph, u32 clk_id,
  119. struct scmi_clock_info *clk)
  120. {
  121. u64 *rate = NULL;
  122. int ret, cnt;
  123. bool rate_discrete = false;
  124. u32 tot_rate_cnt = 0, rates_flag;
  125. u16 num_returned, num_remaining;
  126. struct scmi_xfer *t;
  127. struct scmi_msg_clock_describe_rates *clk_desc;
  128. struct scmi_msg_resp_clock_describe_rates *rlist;
  129. ret = ph->xops->xfer_get_init(ph, CLOCK_DESCRIBE_RATES,
  130. sizeof(*clk_desc), 0, &t);
  131. if (ret)
  132. return ret;
  133. clk_desc = t->tx.buf;
  134. rlist = t->rx.buf;
  135. do {
  136. clk_desc->id = cpu_to_le32(clk_id);
  137. /* Set the number of rates to be skipped/already read */
  138. clk_desc->rate_index = cpu_to_le32(tot_rate_cnt);
  139. ret = ph->xops->do_xfer(ph, t);
  140. if (ret)
  141. goto err;
  142. rates_flag = le32_to_cpu(rlist->num_rates_flags);
  143. num_remaining = NUM_REMAINING(rates_flag);
  144. rate_discrete = RATE_DISCRETE(rates_flag);
  145. num_returned = NUM_RETURNED(rates_flag);
  146. if (tot_rate_cnt + num_returned > SCMI_MAX_NUM_RATES) {
  147. dev_err(ph->dev, "No. of rates > MAX_NUM_RATES");
  148. break;
  149. }
  150. if (!rate_discrete) {
  151. clk->range.min_rate = RATE_TO_U64(rlist->rate[0]);
  152. clk->range.max_rate = RATE_TO_U64(rlist->rate[1]);
  153. clk->range.step_size = RATE_TO_U64(rlist->rate[2]);
  154. dev_dbg(ph->dev, "Min %llu Max %llu Step %llu Hz\n",
  155. clk->range.min_rate, clk->range.max_rate,
  156. clk->range.step_size);
  157. break;
  158. }
  159. rate = &clk->list.rates[tot_rate_cnt];
  160. for (cnt = 0; cnt < num_returned; cnt++, rate++) {
  161. *rate = RATE_TO_U64(rlist->rate[cnt]);
  162. dev_dbg(ph->dev, "Rate %llu Hz\n", *rate);
  163. }
  164. tot_rate_cnt += num_returned;
  165. ph->xops->reset_rx_to_maxsz(ph, t);
  166. /*
  167. * check for both returned and remaining to avoid infinite
  168. * loop due to buggy firmware
  169. */
  170. } while (num_returned && num_remaining);
  171. if (rate_discrete && rate) {
  172. clk->list.num_rates = tot_rate_cnt;
  173. sort(clk->list.rates, tot_rate_cnt, sizeof(*rate),
  174. rate_cmp_func, NULL);
  175. }
  176. clk->rate_discrete = rate_discrete;
  177. err:
  178. ph->xops->xfer_put(ph, t);
  179. return ret;
  180. }
  181. static int
  182. scmi_clock_rate_get(const struct scmi_protocol_handle *ph,
  183. u32 clk_id, u64 *value)
  184. {
  185. int ret;
  186. struct scmi_xfer *t;
  187. ret = ph->xops->xfer_get_init(ph, CLOCK_RATE_GET,
  188. sizeof(__le32), sizeof(u64), &t);
  189. if (ret)
  190. return ret;
  191. put_unaligned_le32(clk_id, t->tx.buf);
  192. ret = ph->xops->do_xfer(ph, t);
  193. if (!ret)
  194. *value = get_unaligned_le64(t->rx.buf);
  195. ph->xops->xfer_put(ph, t);
  196. return ret;
  197. }
  198. static int scmi_clock_rate_set(const struct scmi_protocol_handle *ph,
  199. u32 clk_id, u64 rate)
  200. {
  201. int ret;
  202. u32 flags = 0;
  203. struct scmi_xfer *t;
  204. struct scmi_clock_set_rate *cfg;
  205. struct clock_info *ci = ph->get_priv(ph);
  206. ret = ph->xops->xfer_get_init(ph, CLOCK_RATE_SET, sizeof(*cfg), 0, &t);
  207. if (ret)
  208. return ret;
  209. if (ci->max_async_req &&
  210. atomic_inc_return(&ci->cur_async_req) < ci->max_async_req)
  211. flags |= CLOCK_SET_ASYNC;
  212. cfg = t->tx.buf;
  213. cfg->flags = cpu_to_le32(flags);
  214. cfg->id = cpu_to_le32(clk_id);
  215. cfg->value_low = cpu_to_le32(rate & 0xffffffff);
  216. cfg->value_high = cpu_to_le32(rate >> 32);
  217. if (flags & CLOCK_SET_ASYNC)
  218. ret = ph->xops->do_xfer_with_response(ph, t);
  219. else
  220. ret = ph->xops->do_xfer(ph, t);
  221. if (ci->max_async_req)
  222. atomic_dec(&ci->cur_async_req);
  223. ph->xops->xfer_put(ph, t);
  224. return ret;
  225. }
  226. static int
  227. scmi_clock_config_set(const struct scmi_protocol_handle *ph, u32 clk_id,
  228. u32 config)
  229. {
  230. int ret;
  231. struct scmi_xfer *t;
  232. struct scmi_clock_set_config *cfg;
  233. ret = ph->xops->xfer_get_init(ph, CLOCK_CONFIG_SET,
  234. sizeof(*cfg), 0, &t);
  235. if (ret)
  236. return ret;
  237. cfg = t->tx.buf;
  238. cfg->id = cpu_to_le32(clk_id);
  239. cfg->attributes = cpu_to_le32(config);
  240. ret = ph->xops->do_xfer(ph, t);
  241. ph->xops->xfer_put(ph, t);
  242. return ret;
  243. }
  244. static int scmi_clock_enable(const struct scmi_protocol_handle *ph, u32 clk_id)
  245. {
  246. return scmi_clock_config_set(ph, clk_id, CLOCK_ENABLE);
  247. }
  248. static int scmi_clock_disable(const struct scmi_protocol_handle *ph, u32 clk_id)
  249. {
  250. return scmi_clock_config_set(ph, clk_id, 0);
  251. }
  252. static int scmi_clock_count_get(const struct scmi_protocol_handle *ph)
  253. {
  254. struct clock_info *ci = ph->get_priv(ph);
  255. return ci->num_clocks;
  256. }
  257. static const struct scmi_clock_info *
  258. scmi_clock_info_get(const struct scmi_protocol_handle *ph, u32 clk_id)
  259. {
  260. struct clock_info *ci = ph->get_priv(ph);
  261. struct scmi_clock_info *clk = ci->clk + clk_id;
  262. if (!clk->name[0])
  263. return NULL;
  264. return clk;
  265. }
  266. static const struct scmi_clk_proto_ops clk_proto_ops = {
  267. .count_get = scmi_clock_count_get,
  268. .info_get = scmi_clock_info_get,
  269. .rate_get = scmi_clock_rate_get,
  270. .rate_set = scmi_clock_rate_set,
  271. .enable = scmi_clock_enable,
  272. .disable = scmi_clock_disable,
  273. };
  274. static int scmi_clock_protocol_init(const struct scmi_protocol_handle *ph)
  275. {
  276. u32 version;
  277. int clkid, ret;
  278. struct clock_info *cinfo;
  279. ph->xops->version_get(ph, &version);
  280. dev_dbg(ph->dev, "Clock Version %d.%d\n",
  281. PROTOCOL_REV_MAJOR(version), PROTOCOL_REV_MINOR(version));
  282. cinfo = devm_kzalloc(ph->dev, sizeof(*cinfo), GFP_KERNEL);
  283. if (!cinfo)
  284. return -ENOMEM;
  285. scmi_clock_protocol_attributes_get(ph, cinfo);
  286. cinfo->clk = devm_kcalloc(ph->dev, cinfo->num_clocks,
  287. sizeof(*cinfo->clk), GFP_KERNEL);
  288. if (!cinfo->clk)
  289. return -ENOMEM;
  290. for (clkid = 0; clkid < cinfo->num_clocks; clkid++) {
  291. struct scmi_clock_info *clk = cinfo->clk + clkid;
  292. ret = scmi_clock_attributes_get(ph, clkid, clk);
  293. if (!ret)
  294. scmi_clock_describe_rates_get(ph, clkid, clk);
  295. }
  296. cinfo->version = version;
  297. return ph->set_priv(ph, cinfo);
  298. }
  299. static const struct scmi_protocol scmi_clock = {
  300. .id = SCMI_PROTOCOL_CLOCK,
  301. .owner = THIS_MODULE,
  302. .init_instance = &scmi_clock_protocol_init,
  303. .ops = &clk_proto_ops,
  304. };
  305. DEFINE_SCMI_PROTOCOL_REGISTER_UNREGISTER(clock, scmi_clock)