dp_mst_sim.c 37 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677167816791680168116821683168416851686
  1. /*
  2. * Copyright (c) 2021-2022 Qualcomm Innovation Center, Inc. All rights reserved.
  3. * Copyright (c) 2019-2021, The Linux Foundation. All rights reserved.
  4. *
  5. * This program is free software; you can redistribute it and/or modify
  6. * it under the terms of the GNU General Public License version 2 and
  7. * only version 2 as published by the Free Software Foundation.
  8. *
  9. * This program is distributed in the hope that it will be useful,
  10. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  12. * GNU General Public License for more details.
  13. *
  14. */
  15. #include <linux/module.h>
  16. #include <linux/slab.h>
  17. #include <linux/uaccess.h>
  18. #include <linux/debugfs.h>
  19. #include <linux/platform_device.h>
  20. #include <drm/drm_edid.h>
  21. #include <drm/drm_dp_helper.h>
  22. #include "dp_debug.h"
  23. #include "dp_mst_sim.h"
  24. struct dp_sim_dpcd_reg {
  25. struct list_head head;
  26. u32 addr;
  27. u8 val;
  28. };
  29. #define DP_SIM_BRIDGE_PRIV_FLAG (1 << 31)
  30. #define MAX_BUILTIN_DPCD_ADDR SZ_2K
  31. #define MAX_MST_PORT 8
  32. struct dp_sim_device {
  33. struct device *dev;
  34. struct dp_aux_bridge bridge;
  35. void *host_dev;
  36. int (*hpd_cb)(void *, bool, bool);
  37. struct mutex lock;
  38. const char *label;
  39. struct dentry *debugfs_dir;
  40. struct dentry *debugfs_edid_dir;
  41. u8 dpcd_reg[MAX_BUILTIN_DPCD_ADDR];
  42. struct list_head dpcd_reg_list;
  43. u32 dpcd_write_addr;
  44. u32 dpcd_write_size;
  45. u32 link_training_cnt;
  46. u32 link_training_remain;
  47. u32 link_training_lane_cnt;
  48. bool link_training_mismatch;
  49. struct dp_mst_sim_port *ports;
  50. u32 port_num;
  51. u32 current_port_num;
  52. u32 sim_mode;
  53. u32 edid_seg;
  54. u32 edid_seg_int;
  55. u32 edid_addr;
  56. bool skip_edid;
  57. bool skip_dpcd;
  58. bool skip_link_training;
  59. bool skip_config;
  60. bool skip_hpd;
  61. bool skip_mst;
  62. };
  63. struct dp_sim_debug_edid_entry {
  64. struct dp_sim_device *sim_dev;
  65. u32 index;
  66. };
  67. #define to_dp_sim_dev(x) container_of((x), struct dp_sim_device, bridge)
  68. static const struct dp_mst_sim_port output_port = {
  69. false, false, true, 3, false, 0x12,
  70. {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  71. 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
  72. 0, 0, 2520, 2520, NULL, 0
  73. };
  74. #if IS_ENABLED(CONFIG_DYNAMIC_DEBUG)
  75. static void dp_sim_aux_hex_dump(struct drm_dp_aux_msg *msg)
  76. {
  77. char prefix[64];
  78. int i, linelen, remaining = msg->size;
  79. const int rowsize = 16;
  80. u8 linebuf[64];
  81. snprintf(prefix, sizeof(prefix), "%s %s %4xh(%2zu): ",
  82. (msg->request & DP_AUX_I2C_MOT) ? "I2C" : "NAT",
  83. (msg->request & DP_AUX_I2C_READ) ? "RD" : "WR",
  84. msg->address, msg->size);
  85. for (i = 0; i < msg->size; i += rowsize) {
  86. linelen = min(remaining, rowsize);
  87. remaining -= rowsize;
  88. hex_dump_to_buffer(msg->buffer + i, linelen, rowsize, 1,
  89. linebuf, sizeof(linebuf), false);
  90. DP_DEBUG("%s%s\n", prefix, linebuf);
  91. }
  92. }
  93. #else
  94. static void dp_sim_aux_hex_dump(struct drm_dp_aux_msg *msg)
  95. {
  96. }
  97. #endif /* CONFIG_DYNAMIC_DEBUG */
  98. static int dp_sim_register_hpd(struct dp_aux_bridge *bridge,
  99. int (*hpd_cb)(void *, bool, bool), void *dev)
  100. {
  101. struct dp_sim_device *sim_dev = to_dp_sim_dev(bridge);
  102. sim_dev->host_dev = dev;
  103. sim_dev->hpd_cb = hpd_cb;
  104. if (sim_dev->skip_hpd)
  105. hpd_cb(dev, true, false);
  106. return 0;
  107. }
  108. static u8 dp_sim_read_dpcd(struct dp_sim_device *sim_dev,
  109. u32 addr)
  110. {
  111. struct dp_sim_dpcd_reg *reg;
  112. if (addr < MAX_BUILTIN_DPCD_ADDR) {
  113. return sim_dev->dpcd_reg[addr];
  114. } else {
  115. list_for_each_entry(reg, &sim_dev->dpcd_reg_list, head) {
  116. if (reg->addr == addr)
  117. return reg->val;
  118. }
  119. }
  120. return 0;
  121. }
  122. static void dp_sim_write_dpcd(struct dp_sim_device *sim_dev,
  123. u32 addr, u8 val)
  124. {
  125. struct dp_sim_dpcd_reg *dpcd_reg;
  126. if (addr < MAX_BUILTIN_DPCD_ADDR) {
  127. sim_dev->dpcd_reg[addr] = val;
  128. } else {
  129. list_for_each_entry(dpcd_reg, &sim_dev->dpcd_reg_list, head) {
  130. if (dpcd_reg->addr == addr) {
  131. dpcd_reg->val = val;
  132. return;
  133. }
  134. }
  135. dpcd_reg = devm_kzalloc(sim_dev->dev,
  136. sizeof(*dpcd_reg), GFP_KERNEL);
  137. if (!dpcd_reg)
  138. return;
  139. dpcd_reg->addr = addr;
  140. dpcd_reg->val = val;
  141. list_add_tail(&dpcd_reg->head, &sim_dev->dpcd_reg_list);
  142. }
  143. }
  144. static int dp_sim_read_dpcd_regs(struct dp_sim_device *sim_dev,
  145. u8 *buf, u32 size, u32 offset)
  146. {
  147. u32 i;
  148. if (offset + size <= MAX_BUILTIN_DPCD_ADDR) {
  149. memcpy(buf, &sim_dev->dpcd_reg[offset], size);
  150. } else {
  151. for (i = 0; i < size; i++)
  152. buf[i] = dp_sim_read_dpcd(sim_dev, offset + i);
  153. }
  154. return size;
  155. }
  156. static int dp_sim_read_edid(struct dp_sim_device *sim_dev,
  157. struct drm_dp_aux_msg *msg)
  158. {
  159. u8 *buf = (u8 *)msg->buffer;
  160. u32 addr;
  161. if (!sim_dev->port_num || !msg->size)
  162. return 0;
  163. if (msg->request & DP_AUX_I2C_READ) {
  164. addr = (sim_dev->edid_seg_int << 8) + sim_dev->edid_addr;
  165. if (addr + msg->size <= sim_dev->ports[0].edid_size) {
  166. memcpy(msg->buffer, &sim_dev->ports[0].edid[addr],
  167. msg->size);
  168. } else if (addr < sim_dev->ports[0].edid_size) {
  169. memcpy(msg->buffer, &sim_dev->ports[0].edid[addr],
  170. sim_dev->ports[0].edid_size - addr);
  171. }
  172. sim_dev->edid_addr += msg->size;
  173. sim_dev->edid_addr &= 0xFF;
  174. } else {
  175. if (msg->address == 0x30) {
  176. sim_dev->edid_seg = buf[0];
  177. } else if (msg->address == 0x50) {
  178. sim_dev->edid_seg_int = sim_dev->edid_seg;
  179. sim_dev->edid_addr = buf[0];
  180. sim_dev->edid_seg = 0;
  181. }
  182. }
  183. return msg->size;
  184. }
  185. static int dp_sim_link_training(struct dp_sim_device *sim_dev,
  186. struct drm_dp_aux *drm_aux,
  187. struct drm_dp_aux_msg *msg)
  188. {
  189. u8 *link_status = msg->buffer;
  190. int ret, i;
  191. if (msg->request == DP_AUX_NATIVE_READ &&
  192. msg->address == DP_LANE0_1_STATUS) {
  193. /*
  194. * remain is an option to allow limited actual
  195. * link training. this is needed for some device
  196. * when actual read is needed.
  197. */
  198. if (sim_dev->link_training_remain) {
  199. sim_dev->link_training_remain--;
  200. ret = drm_aux->transfer(drm_aux, msg);
  201. if (ret >= 0)
  202. link_status[2] &= ~DP_LINK_STATUS_UPDATED;
  203. return ret;
  204. }
  205. memcpy(msg->buffer, &sim_dev->dpcd_reg[msg->address],
  206. msg->size);
  207. /*
  208. * when mismatch happens, clear status and fail the link
  209. * training.
  210. */
  211. if (sim_dev->link_training_mismatch) {
  212. link_status[0] = 0;
  213. link_status[1] = 0;
  214. }
  215. return msg->size;
  216. }
  217. if (msg->request == DP_AUX_NATIVE_WRITE) {
  218. if (msg->address == DP_TRAINING_LANE0_SET) {
  219. const u8 mask = DP_TRAIN_VOLTAGE_SWING_MASK |
  220. DP_TRAIN_PRE_EMPHASIS_MASK;
  221. /*
  222. * when link training is set, only pre-set vx/px is
  223. * going through. here we will fail the initial
  224. * vx/px and correct them automatically.
  225. */
  226. sim_dev->link_training_mismatch = false;
  227. for (i = 0; i < sim_dev->link_training_lane_cnt; i++) {
  228. if ((link_status[i] & mask) !=
  229. (sim_dev->dpcd_reg[
  230. DP_TRAINING_LANE0_SET + i] & mask)) {
  231. sim_dev->link_training_mismatch = true;
  232. break;
  233. }
  234. }
  235. } else if (msg->address == DP_TRAINING_PATTERN_SET) {
  236. sim_dev->link_training_remain =
  237. sim_dev->link_training_cnt;
  238. } else if (msg->address == DP_LINK_BW_SET) {
  239. sim_dev->link_training_lane_cnt =
  240. link_status[1] & 0x1F;
  241. }
  242. }
  243. return 0;
  244. }
  245. static ssize_t dp_sim_transfer(struct dp_aux_bridge *bridge,
  246. struct drm_dp_aux *drm_aux,
  247. struct drm_dp_aux_msg *msg)
  248. {
  249. struct dp_sim_device *sim_dev = to_dp_sim_dev(bridge);
  250. int ret;
  251. mutex_lock(&sim_dev->lock);
  252. if (sim_dev->skip_link_training &&
  253. !(sim_dev->sim_mode & DP_SIM_MODE_LINK_TRAIN)) {
  254. ret = dp_sim_link_training(sim_dev, drm_aux, msg);
  255. if (ret)
  256. goto end;
  257. }
  258. if ((sim_dev->sim_mode & DP_SIM_MODE_MST) || sim_dev->skip_mst) {
  259. ret = dp_mst_sim_transfer(sim_dev->bridge.mst_ctx, msg);
  260. if (ret >= 0) {
  261. ret = msg->size;
  262. goto end;
  263. }
  264. }
  265. if (msg->request == DP_AUX_NATIVE_WRITE) {
  266. sim_dev->dpcd_write_addr = msg->address;
  267. sim_dev->dpcd_write_size = msg->size;
  268. }
  269. if (((sim_dev->sim_mode & DP_SIM_MODE_EDID) ||
  270. sim_dev->skip_edid) &&
  271. (msg->request & DP_AUX_I2C_MOT))
  272. ret = dp_sim_read_edid(sim_dev, msg);
  273. else if (((sim_dev->sim_mode & DP_SIM_MODE_DPCD_READ) ||
  274. sim_dev->skip_dpcd) &&
  275. msg->request == DP_AUX_NATIVE_READ)
  276. ret = dp_sim_read_dpcd_regs(sim_dev, msg->buffer,
  277. msg->size, msg->address);
  278. else if (((sim_dev->sim_mode & DP_SIM_MODE_DPCD_WRITE) ||
  279. sim_dev->skip_config) &&
  280. msg->request == DP_AUX_NATIVE_WRITE)
  281. ret = msg->size;
  282. else
  283. ret = drm_aux->transfer(drm_aux, msg);
  284. end:
  285. dp_sim_aux_hex_dump(msg);
  286. mutex_unlock(&sim_dev->lock);
  287. return ret;
  288. }
  289. static void dp_sim_host_hpd_irq(void *host_dev)
  290. {
  291. struct dp_sim_device *sim_dev = host_dev;
  292. if (sim_dev->hpd_cb)
  293. sim_dev->hpd_cb(sim_dev->host_dev, true, true);
  294. }
  295. int dp_sim_set_sim_mode(struct dp_aux_bridge *bridge, u32 sim_mode)
  296. {
  297. struct dp_sim_device *sim_dev;
  298. if (!bridge || !(bridge->flag & DP_SIM_BRIDGE_PRIV_FLAG))
  299. return -EINVAL;
  300. sim_dev = to_dp_sim_dev(bridge);
  301. sim_dev->sim_mode = sim_mode;
  302. return 0;
  303. }
  304. int dp_sim_update_port_num(struct dp_aux_bridge *bridge, u32 port_num)
  305. {
  306. struct dp_sim_device *sim_dev;
  307. struct dp_mst_sim_port *ports;
  308. u32 i, rc;
  309. if (!bridge || !(bridge->flag & DP_SIM_BRIDGE_PRIV_FLAG))
  310. return -EINVAL;
  311. sim_dev = to_dp_sim_dev(bridge);
  312. if (port_num > sim_dev->port_num) {
  313. ports = devm_kzalloc(sim_dev->dev,
  314. port_num * sizeof(*ports), GFP_KERNEL);
  315. if (!ports)
  316. return -ENOMEM;
  317. memcpy(ports, sim_dev->ports,
  318. sim_dev->port_num * sizeof(*ports));
  319. if (sim_dev->ports)
  320. devm_kfree(sim_dev->dev, sim_dev->ports);
  321. sim_dev->ports = ports;
  322. for (i = sim_dev->port_num; i < port_num; i++) {
  323. memcpy(&ports[i], &output_port, sizeof(*ports));
  324. ports[i].peer_guid[0] = i;
  325. }
  326. sim_dev->port_num = port_num;
  327. }
  328. rc = dp_mst_sim_update(sim_dev->bridge.mst_ctx,
  329. port_num, sim_dev->ports);
  330. if (rc)
  331. return rc;
  332. sim_dev->current_port_num = port_num;
  333. return rc;
  334. }
  335. int dp_sim_update_port_status(struct dp_aux_bridge *bridge,
  336. int port, enum drm_connector_status status)
  337. {
  338. struct dp_sim_device *sim_dev;
  339. if (!bridge || !(bridge->flag & DP_SIM_BRIDGE_PRIV_FLAG))
  340. return -EINVAL;
  341. sim_dev = to_dp_sim_dev(bridge);
  342. if (port < 0 || port >= sim_dev->current_port_num)
  343. return -EINVAL;
  344. sim_dev->ports[port].pdt = (status == connector_status_connected) ?
  345. DP_PEER_DEVICE_SST_SINK : DP_PEER_DEVICE_NONE;
  346. return dp_mst_sim_update(sim_dev->bridge.mst_ctx,
  347. sim_dev->current_port_num, sim_dev->ports);
  348. }
  349. int dp_sim_update_port_edid(struct dp_aux_bridge *bridge,
  350. int port, const u8 *edid, u32 size)
  351. {
  352. struct dp_sim_device *sim_dev;
  353. struct dp_mst_sim_port *sim_port;
  354. if (!bridge || !(bridge->flag & DP_SIM_BRIDGE_PRIV_FLAG))
  355. return -EINVAL;
  356. sim_dev = to_dp_sim_dev(bridge);
  357. if (port < 0 || port >= sim_dev->current_port_num)
  358. return -EINVAL;
  359. sim_port = &sim_dev->ports[port];
  360. if (size != sim_port->edid_size) {
  361. if (sim_port->edid)
  362. devm_kfree(sim_dev->dev, (u8 *)sim_port->edid);
  363. sim_port->edid = devm_kzalloc(sim_dev->dev,
  364. size, GFP_KERNEL);
  365. if (!sim_port->edid)
  366. return -ENOMEM;
  367. sim_port->edid_size = size;
  368. }
  369. memcpy((u8 *)sim_port->edid, edid, size);
  370. return dp_mst_sim_update(sim_dev->bridge.mst_ctx,
  371. sim_dev->current_port_num, sim_dev->ports);
  372. }
  373. int dp_sim_write_dpcd_reg(struct dp_aux_bridge *bridge,
  374. const u8 *dpcd, u32 size, u32 offset)
  375. {
  376. struct dp_sim_device *sim_dev;
  377. int i;
  378. if (!bridge || !(bridge->flag & DP_SIM_BRIDGE_PRIV_FLAG))
  379. return -EINVAL;
  380. sim_dev = to_dp_sim_dev(bridge);
  381. for (i = 0; i < size; i++)
  382. dp_sim_write_dpcd(sim_dev, offset + i, dpcd[i]);
  383. return 0;
  384. }
  385. int dp_sim_read_dpcd_reg(struct dp_aux_bridge *bridge,
  386. u8 *dpcd, u32 size, u32 offset)
  387. {
  388. struct dp_sim_device *sim_dev;
  389. if (!bridge || !(bridge->flag & DP_SIM_BRIDGE_PRIV_FLAG))
  390. return -EINVAL;
  391. sim_dev = to_dp_sim_dev(bridge);
  392. return dp_sim_read_dpcd_regs(sim_dev, dpcd, size, offset);
  393. }
  394. static void dp_sim_update_dtd(struct edid *edid,
  395. struct drm_display_mode *mode)
  396. {
  397. struct detailed_timing *dtd = &edid->detailed_timings[0];
  398. struct detailed_pixel_timing *pd = &dtd->data.pixel_data;
  399. u32 h_blank = mode->htotal - mode->hdisplay;
  400. u32 v_blank = mode->vtotal - mode->vdisplay;
  401. u32 h_img = 0, v_img = 0;
  402. dtd->pixel_clock = cpu_to_le16(mode->clock / 10);
  403. pd->hactive_lo = mode->hdisplay & 0xFF;
  404. pd->hblank_lo = h_blank & 0xFF;
  405. pd->hactive_hblank_hi = ((h_blank >> 8) & 0xF) |
  406. ((mode->hdisplay >> 8) & 0xF) << 4;
  407. pd->vactive_lo = mode->vdisplay & 0xFF;
  408. pd->vblank_lo = v_blank & 0xFF;
  409. pd->vactive_vblank_hi = ((v_blank >> 8) & 0xF) |
  410. ((mode->vdisplay >> 8) & 0xF) << 4;
  411. pd->hsync_offset_lo =
  412. (mode->hsync_start - mode->hdisplay) & 0xFF;
  413. pd->hsync_pulse_width_lo =
  414. (mode->hsync_end - mode->hsync_start) & 0xFF;
  415. pd->vsync_offset_pulse_width_lo =
  416. (((mode->vsync_start - mode->vdisplay) & 0xF) << 4) |
  417. ((mode->vsync_end - mode->vsync_start) & 0xF);
  418. pd->hsync_vsync_offset_pulse_width_hi =
  419. ((((mode->hsync_start - mode->hdisplay) >> 8) & 0x3) << 6) |
  420. ((((mode->hsync_end - mode->hsync_start) >> 8) & 0x3) << 4) |
  421. ((((mode->vsync_start - mode->vdisplay) >> 4) & 0x3) << 2) |
  422. ((((mode->vsync_end - mode->vsync_start) >> 4) & 0x3) << 0);
  423. pd->width_mm_lo = h_img & 0xFF;
  424. pd->height_mm_lo = v_img & 0xFF;
  425. pd->width_height_mm_hi = (((h_img >> 8) & 0xF) << 4) |
  426. ((v_img >> 8) & 0xF);
  427. pd->hborder = 0;
  428. pd->vborder = 0;
  429. pd->misc = 0;
  430. }
  431. static void dp_sim_update_checksum(struct edid *edid)
  432. {
  433. u8 *data = (u8 *)edid;
  434. u32 i, sum = 0;
  435. for (i = 0; i < EDID_LENGTH - 1; i++)
  436. sum += data[i];
  437. edid->checksum = 0x100 - (sum & 0xFF);
  438. }
  439. static int dp_sim_parse_edid_from_node(struct dp_sim_device *sim_dev,
  440. int index, struct device_node *node)
  441. {
  442. struct dp_mst_sim_port *port;
  443. struct drm_display_mode mode_buf, *mode = &mode_buf;
  444. u16 h_front_porch, h_pulse_width, h_back_porch;
  445. u16 v_front_porch, v_pulse_width, v_back_porch;
  446. bool h_active_high, v_active_high;
  447. u32 flags = 0;
  448. int rc;
  449. struct edid *edid;
  450. const u8 edid_buf[EDID_LENGTH] = {
  451. 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x44, 0x6D,
  452. 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x1B, 0x10, 0x01, 0x03,
  453. 0x80, 0x50, 0x2D, 0x78, 0x0A, 0x0D, 0xC9, 0xA0, 0x57, 0x47,
  454. 0x98, 0x27, 0x12, 0x48, 0x4C, 0x00, 0x00, 0x00, 0x01, 0x01,
  455. 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
  456. 0x01, 0x01, 0x01, 0x01,
  457. };
  458. rc = of_property_read_u16(node, "qcom,mode-h-active",
  459. &mode->hdisplay);
  460. if (rc) {
  461. DP_ERR("failed to read h-active, rc=%d\n", rc);
  462. goto fail;
  463. }
  464. rc = of_property_read_u16(node, "qcom,mode-h-front-porch",
  465. &h_front_porch);
  466. if (rc) {
  467. DP_ERR("failed to read h-front-porch, rc=%d\n", rc);
  468. goto fail;
  469. }
  470. rc = of_property_read_u16(node, "qcom,mode-h-pulse-width",
  471. &h_pulse_width);
  472. if (rc) {
  473. DP_ERR("failed to read h-pulse-width, rc=%d\n", rc);
  474. goto fail;
  475. }
  476. rc = of_property_read_u16(node, "qcom,mode-h-back-porch",
  477. &h_back_porch);
  478. if (rc) {
  479. DP_ERR("failed to read h-back-porch, rc=%d\n", rc);
  480. goto fail;
  481. }
  482. h_active_high = of_property_read_bool(node,
  483. "qcom,mode-h-active-high");
  484. rc = of_property_read_u16(node, "qcom,mode-v-active",
  485. &mode->vdisplay);
  486. if (rc) {
  487. DP_ERR("failed to read v-active, rc=%d\n", rc);
  488. goto fail;
  489. }
  490. rc = of_property_read_u16(node, "qcom,mode-v-front-porch",
  491. &v_front_porch);
  492. if (rc) {
  493. DP_ERR("failed to read v-front-porch, rc=%d\n", rc);
  494. goto fail;
  495. }
  496. rc = of_property_read_u16(node, "qcom,mode-v-pulse-width",
  497. &v_pulse_width);
  498. if (rc) {
  499. DP_ERR("failed to read v-pulse-width, rc=%d\n", rc);
  500. goto fail;
  501. }
  502. rc = of_property_read_u16(node, "qcom,mode-v-back-porch",
  503. &v_back_porch);
  504. if (rc) {
  505. DP_ERR("failed to read v-back-porch, rc=%d\n", rc);
  506. goto fail;
  507. }
  508. v_active_high = of_property_read_bool(node,
  509. "qcom,mode-v-active-high");
  510. rc = of_property_read_u32(node, "qcom,mode-clock-in-khz",
  511. &mode->clock);
  512. if (rc) {
  513. DP_ERR("failed to read clock, rc=%d\n", rc);
  514. goto fail;
  515. }
  516. mode->hsync_start = mode->hdisplay + h_front_porch;
  517. mode->hsync_end = mode->hsync_start + h_pulse_width;
  518. mode->htotal = mode->hsync_end + h_back_porch;
  519. mode->vsync_start = mode->vdisplay + v_front_porch;
  520. mode->vsync_end = mode->vsync_start + v_pulse_width;
  521. mode->vtotal = mode->vsync_end + v_back_porch;
  522. if (h_active_high)
  523. flags |= DRM_MODE_FLAG_PHSYNC;
  524. else
  525. flags |= DRM_MODE_FLAG_NHSYNC;
  526. if (v_active_high)
  527. flags |= DRM_MODE_FLAG_PVSYNC;
  528. else
  529. flags |= DRM_MODE_FLAG_NVSYNC;
  530. mode->flags = flags;
  531. edid = devm_kzalloc(sim_dev->dev, sizeof(*edid), GFP_KERNEL);
  532. if (!edid) {
  533. rc = -ENOMEM;
  534. goto fail;
  535. }
  536. memcpy(edid, edid_buf, sizeof(edid_buf));
  537. dp_sim_update_dtd(edid, mode);
  538. dp_sim_update_checksum(edid);
  539. port = &sim_dev->ports[index];
  540. memcpy(port, &output_port, sizeof(*port));
  541. port->peer_guid[0] = index;
  542. if (port->edid)
  543. devm_kfree(sim_dev->dev, (u8 *)port->edid);
  544. port->edid = (u8 *)edid;
  545. port->edid_size = sizeof(*edid);
  546. fail:
  547. return rc;
  548. }
  549. static int dp_sim_parse_edid_from_data(struct dp_sim_device *sim_dev,
  550. int index, const char *data, int len)
  551. {
  552. struct dp_mst_sim_port *port;
  553. u8 *edid_data;
  554. edid_data = devm_kzalloc(sim_dev->dev, len, GFP_KERNEL);
  555. if (!edid_data)
  556. return -ENOMEM;
  557. memcpy(edid_data, data, len);
  558. port = &sim_dev->ports[index];
  559. memcpy(port, &output_port, sizeof(*port));
  560. port->peer_guid[0] = index;
  561. if (port->edid)
  562. devm_kfree(sim_dev->dev, (u8 *)port->edid);
  563. port->edid = edid_data;
  564. port->edid_size = len;
  565. return 0;
  566. }
  567. static int dp_sim_parse_edid(struct dp_sim_device *sim_dev)
  568. {
  569. struct dp_mst_sim_port *ports;
  570. struct device_node *of_node = sim_dev->bridge.of_node;
  571. struct device_node *node;
  572. const char *data;
  573. int rc, port_num, i, len;
  574. port_num = of_get_child_count(of_node);
  575. if (!port_num)
  576. port_num = 1;
  577. if (port_num >= 15)
  578. return -EINVAL;
  579. ports = devm_kzalloc(sim_dev->dev,
  580. port_num * sizeof(*ports), GFP_KERNEL);
  581. if (!ports)
  582. return -ENOMEM;
  583. sim_dev->ports = ports;
  584. sim_dev->port_num = port_num;
  585. sim_dev->current_port_num = port_num;
  586. i = 0;
  587. for_each_child_of_node(of_node, node) {
  588. data = of_get_property(node, "qcom,edid", &len);
  589. if (data)
  590. rc = dp_sim_parse_edid_from_data(sim_dev, i,
  591. data, len);
  592. else
  593. rc = dp_sim_parse_edid_from_node(sim_dev, i,
  594. node);
  595. if (rc)
  596. return rc;
  597. i++;
  598. }
  599. if (i == 0)
  600. memcpy(ports, &output_port, sizeof(*ports));
  601. return 0;
  602. }
  603. static int dp_sim_parse_dpcd(struct dp_sim_device *sim_dev)
  604. {
  605. struct device_node *node = sim_dev->bridge.of_node;
  606. u32 val, i;
  607. const __be32 *arr;
  608. int rc;
  609. rc = of_property_read_u32(node, "qcom,dpcd-max-rate", &val);
  610. if (!rc)
  611. sim_dev->dpcd_reg[DP_MAX_LINK_RATE] = val;
  612. rc = of_property_read_u32(node, "qcom,dpcd-max-lane", &val);
  613. if (!rc)
  614. sim_dev->dpcd_reg[DP_MAX_LANE_COUNT] = val;
  615. rc = of_property_read_u32(node, "qcom,dpcd-mst", &val);
  616. if (!rc)
  617. sim_dev->dpcd_reg[DP_MSTM_CAP] = val;
  618. arr = of_get_property(node, "qcom,dpcd-regs", &val);
  619. if (arr) {
  620. val /= sizeof(u32);
  621. val &= ~0x1;
  622. for (i = 0; i < val; i += 2)
  623. dp_sim_write_dpcd(sim_dev,
  624. be32_to_cpu(arr[i]),
  625. be32_to_cpu(arr[i+1]));
  626. }
  627. rc = of_property_read_u32(node, "qcom,voltage-swing", &val);
  628. if (!rc)
  629. for (i = 0; i < 4; i++) {
  630. sim_dev->dpcd_reg[DP_TRAINING_LANE0_SET + i] |=
  631. val;
  632. sim_dev->dpcd_reg[DP_ADJUST_REQUEST_LANE0_1 + (i/2)] |=
  633. (val & 0x3) << ((i & 0x1) << 2);
  634. }
  635. rc = of_property_read_u32(node, "qcom,pre-emphasis", &val);
  636. if (!rc)
  637. for (i = 0; i < 4; i++) {
  638. sim_dev->dpcd_reg[DP_TRAINING_LANE0_SET + i] |=
  639. val << 3;
  640. sim_dev->dpcd_reg[DP_ADJUST_REQUEST_LANE0_1 + (i/2)] |=
  641. (val & 0x3) << (((i & 0x1) << 2) + 2);
  642. }
  643. rc = of_property_read_u32(node, "qcom,link-training-cnt", &val);
  644. if (!rc)
  645. sim_dev->link_training_cnt = val;
  646. else
  647. sim_dev->link_training_cnt = 0;
  648. return 0;
  649. }
  650. static int dp_sim_parse_misc(struct dp_sim_device *sim_dev)
  651. {
  652. struct device_node *node = sim_dev->bridge.of_node;
  653. sim_dev->skip_edid = of_property_read_bool(node,
  654. "qcom,skip-edid");
  655. sim_dev->skip_dpcd = of_property_read_bool(node,
  656. "qcom,skip-dpcd-read");
  657. sim_dev->skip_link_training = of_property_read_bool(node,
  658. "qcom,skip-link-training");
  659. sim_dev->skip_config = of_property_read_bool(node,
  660. "qcom,skip-dpcd-write");
  661. sim_dev->skip_hpd = of_property_read_bool(node,
  662. "qcom,skip-hpd");
  663. sim_dev->skip_mst = of_property_read_bool(node,
  664. "qcom,skip-mst");
  665. DP_DEBUG("skip: edid=%d dpcd=%d LT=%d config=%d hpd=%d mst=%d\n",
  666. sim_dev->skip_edid,
  667. sim_dev->skip_dpcd,
  668. sim_dev->skip_link_training,
  669. sim_dev->skip_config,
  670. sim_dev->skip_hpd,
  671. sim_dev->skip_mst);
  672. return 0;
  673. }
  674. static ssize_t dp_sim_debug_write_edid(struct file *file,
  675. const char __user *user_buff, size_t count, loff_t *ppos)
  676. {
  677. struct dp_sim_debug_edid_entry *entry = file->private_data;
  678. struct dp_sim_device *debug;
  679. struct dp_mst_sim_port *port;
  680. u8 *buf = NULL, *buf_t = NULL;
  681. const int char_to_nib = 2;
  682. size_t edid_size = 0;
  683. size_t size = 0, edid_buf_index = 0;
  684. ssize_t rc = count;
  685. if (!entry)
  686. return -ENODEV;
  687. debug = entry->sim_dev;
  688. if (!debug || entry->index >= debug->port_num)
  689. return -EINVAL;
  690. port = &debug->ports[entry->index];
  691. mutex_lock(&debug->lock);
  692. if (*ppos)
  693. goto bail;
  694. size = min_t(size_t, count, SZ_1K);
  695. buf = kzalloc(size, GFP_KERNEL);
  696. if (ZERO_OR_NULL_PTR(buf)) {
  697. rc = -ENOMEM;
  698. goto bail;
  699. }
  700. if (copy_from_user(buf, user_buff, size))
  701. goto bail;
  702. edid_size = size / char_to_nib;
  703. buf_t = buf;
  704. if (edid_size != port->edid_size) {
  705. if (port->edid)
  706. devm_kfree(debug->dev, (u8 *)port->edid);
  707. port->edid = devm_kzalloc(debug->dev,
  708. edid_size, GFP_KERNEL);
  709. if (!port->edid) {
  710. rc = -ENOMEM;
  711. goto bail;
  712. }
  713. port->edid_size = edid_size;
  714. }
  715. while (edid_size--) {
  716. char t[3];
  717. int d;
  718. memcpy(t, buf_t, sizeof(char) * char_to_nib);
  719. t[char_to_nib] = '\0';
  720. if (kstrtoint(t, 16, &d)) {
  721. DP_ERR("kstrtoint error\n");
  722. goto bail;
  723. }
  724. if (port->edid && (edid_buf_index < port->edid_size))
  725. ((u8 *)port->edid)[edid_buf_index++] = d;
  726. buf_t += char_to_nib;
  727. }
  728. if (debug->skip_mst)
  729. dp_mst_sim_update(debug->bridge.mst_ctx,
  730. debug->port_num, debug->ports);
  731. debug->skip_edid = true;
  732. bail:
  733. kfree(buf);
  734. mutex_unlock(&debug->lock);
  735. return rc;
  736. }
  737. static ssize_t dp_sim_debug_write_dpcd(struct file *file,
  738. const char __user *user_buff, size_t count, loff_t *ppos)
  739. {
  740. struct dp_sim_device *debug = file->private_data;
  741. u8 *buf = NULL, *buf_t = NULL;
  742. const int char_to_nib = 2;
  743. size_t dpcd_size = 0;
  744. size_t size = 0, dpcd_buf_index = 0;
  745. ssize_t rc = count;
  746. char offset_ch[5];
  747. u32 offset, data_len;
  748. if (!debug)
  749. return -ENODEV;
  750. mutex_lock(&debug->lock);
  751. if (*ppos)
  752. goto bail;
  753. size = min_t(size_t, count, SZ_2K);
  754. if (size < 4)
  755. goto bail;
  756. buf = kzalloc(size, GFP_KERNEL);
  757. if (ZERO_OR_NULL_PTR(buf)) {
  758. rc = -ENOMEM;
  759. goto bail;
  760. }
  761. if (copy_from_user(buf, user_buff, size))
  762. goto bail;
  763. memcpy(offset_ch, buf, 4);
  764. offset_ch[4] = '\0';
  765. if (kstrtoint(offset_ch, 16, &offset)) {
  766. DP_ERR("offset kstrtoint error\n");
  767. goto bail;
  768. }
  769. if (offset == 0xFFFF) {
  770. DP_ERR("clearing dpcd\n");
  771. memset(debug->dpcd_reg, 0, sizeof(debug->dpcd_reg));
  772. goto bail;
  773. }
  774. size -= 4;
  775. if (size == 0)
  776. goto bail;
  777. dpcd_size = size / char_to_nib;
  778. data_len = dpcd_size;
  779. buf_t = buf + 4;
  780. dpcd_buf_index = offset;
  781. while (dpcd_size--) {
  782. char t[3];
  783. int d;
  784. memcpy(t, buf_t, sizeof(char) * char_to_nib);
  785. t[char_to_nib] = '\0';
  786. if (kstrtoint(t, 16, &d)) {
  787. DP_ERR("kstrtoint error\n");
  788. goto bail;
  789. }
  790. dp_sim_write_dpcd(debug, dpcd_buf_index, d);
  791. dpcd_buf_index++;
  792. buf_t += char_to_nib;
  793. }
  794. debug->skip_dpcd = true;
  795. debug->skip_config = true;
  796. bail:
  797. kfree(buf);
  798. mutex_unlock(&debug->lock);
  799. return rc;
  800. }
  801. static ssize_t dp_sim_debug_read_dpcd(struct file *file,
  802. char __user *user_buff, size_t count, loff_t *ppos)
  803. {
  804. struct dp_sim_device *debug = file->private_data;
  805. char *buf;
  806. int const buf_size = SZ_4K;
  807. u32 offset = 0;
  808. u32 len = 0;
  809. if (!debug)
  810. return -ENODEV;
  811. if (*ppos)
  812. return 0;
  813. buf = kzalloc(buf_size, GFP_KERNEL);
  814. if (!buf)
  815. return -ENOMEM;
  816. len += snprintf(buf, buf_size, "0x%x", debug->dpcd_write_addr);
  817. while (1) {
  818. if (debug->dpcd_write_addr + offset >= buf_size ||
  819. offset >= debug->dpcd_write_size)
  820. break;
  821. len += snprintf(buf + len, buf_size - len, "0x%x",
  822. debug->dpcd_reg[debug->dpcd_write_addr + offset++]);
  823. }
  824. len = min_t(size_t, count, len);
  825. if (!copy_to_user(user_buff, buf, len))
  826. *ppos += len;
  827. kfree(buf);
  828. return len;
  829. }
  830. static ssize_t dp_sim_debug_write_hpd(struct file *file,
  831. const char __user *user_buff, size_t count, loff_t *ppos)
  832. {
  833. struct dp_sim_device *debug = file->private_data;
  834. char buf[SZ_8];
  835. size_t len = 0;
  836. int hpd = 0;
  837. if (!debug)
  838. return -ENODEV;
  839. if (*ppos)
  840. return 0;
  841. len = min_t(size_t, count, SZ_8 - 1);
  842. if (copy_from_user(buf, user_buff, len))
  843. goto end;
  844. buf[len] = '\0';
  845. if (kstrtoint(buf, 10, &hpd) != 0)
  846. goto end;
  847. if (debug->hpd_cb)
  848. debug->hpd_cb(debug->host_dev, !!hpd, false);
  849. end:
  850. return len;
  851. }
  852. static ssize_t dp_sim_debug_write_skip_link_training(struct file *file,
  853. const char __user *user_buff, size_t count, loff_t *ppos)
  854. {
  855. struct dp_sim_device *debug = file->private_data;
  856. char buf[SZ_8];
  857. size_t len = 0;
  858. int skip_lk, lk_cnt;
  859. if (!debug)
  860. return -ENODEV;
  861. if (*ppos)
  862. return 0;
  863. len = min_t(size_t, count, SZ_8 - 1);
  864. if (copy_from_user(buf, user_buff, len))
  865. goto end;
  866. buf[len] = '\0';
  867. if (sscanf(buf, "%d %u", &skip_lk, &lk_cnt) != 2) {
  868. DP_ERR("invalid input\n");
  869. return -EINVAL;
  870. }
  871. mutex_lock(&debug->lock);
  872. debug->skip_link_training = !!skip_lk;
  873. debug->link_training_cnt = lk_cnt;
  874. mutex_unlock(&debug->lock);
  875. end:
  876. return len;
  877. }
  878. static ssize_t dp_sim_debug_write_skip_edid(struct file *file,
  879. const char __user *user_buff, size_t count, loff_t *ppos)
  880. {
  881. struct dp_sim_device *debug = file->private_data;
  882. char buf[SZ_8];
  883. size_t len = 0;
  884. int val = 0;
  885. if (!debug)
  886. return -ENODEV;
  887. if (*ppos)
  888. return 0;
  889. len = min_t(size_t, count, SZ_8 - 1);
  890. if (copy_from_user(buf, user_buff, len))
  891. goto end;
  892. buf[len] = '\0';
  893. if (kstrtoint(buf, 10, &val) != 0)
  894. goto end;
  895. mutex_lock(&debug->lock);
  896. debug->skip_edid = !!val;
  897. mutex_unlock(&debug->lock);
  898. end:
  899. return len;
  900. }
  901. static ssize_t dp_sim_debug_write_skip_dpcd(struct file *file,
  902. const char __user *user_buff, size_t count, loff_t *ppos)
  903. {
  904. struct dp_sim_device *debug = file->private_data;
  905. char buf[SZ_8];
  906. size_t len = 0;
  907. int val = 0;
  908. if (!debug)
  909. return -ENODEV;
  910. if (*ppos)
  911. return 0;
  912. len = min_t(size_t, count, SZ_8 - 1);
  913. if (copy_from_user(buf, user_buff, len))
  914. goto end;
  915. buf[len] = '\0';
  916. if (kstrtoint(buf, 10, &val) != 0)
  917. goto end;
  918. mutex_lock(&debug->lock);
  919. debug->skip_dpcd = !!val;
  920. mutex_unlock(&debug->lock);
  921. end:
  922. return len;
  923. }
  924. static ssize_t dp_sim_debug_write_skip_config(struct file *file,
  925. const char __user *user_buff, size_t count, loff_t *ppos)
  926. {
  927. struct dp_sim_device *debug = file->private_data;
  928. char buf[SZ_8];
  929. size_t len = 0;
  930. int val = 0;
  931. if (!debug)
  932. return -ENODEV;
  933. if (*ppos)
  934. return 0;
  935. len = min_t(size_t, count, SZ_8 - 1);
  936. if (copy_from_user(buf, user_buff, len))
  937. goto end;
  938. buf[len] = '\0';
  939. if (kstrtoint(buf, 10, &val) != 0)
  940. goto end;
  941. mutex_lock(&debug->lock);
  942. debug->skip_config = !!val;
  943. mutex_unlock(&debug->lock);
  944. end:
  945. return len;
  946. }
  947. static ssize_t dp_sim_debug_write_mst_hpd(struct file *file,
  948. const char __user *user_buff, size_t count, loff_t *ppos)
  949. {
  950. struct dp_sim_debug_edid_entry *entry = file->private_data;
  951. struct dp_sim_device *debug;
  952. char buf[SZ_8];
  953. size_t len = 0;
  954. int hpd = 0;
  955. if (!entry)
  956. return -ENODEV;
  957. debug = entry->sim_dev;
  958. if (!debug || entry->index >= debug->port_num)
  959. return -EINVAL;
  960. if (*ppos)
  961. return 0;
  962. len = min_t(size_t, count, SZ_8 - 1);
  963. if (copy_from_user(buf, user_buff, len))
  964. goto end;
  965. buf[len] = '\0';
  966. if (kstrtoint(buf, 10, &hpd) != 0)
  967. goto end;
  968. dp_sim_update_port_status(&debug->bridge,
  969. entry->index, hpd ?
  970. connector_status_connected :
  971. connector_status_disconnected);
  972. end:
  973. return len;
  974. }
  975. static const struct file_operations sim_edid_fops = {
  976. .open = simple_open,
  977. .write = dp_sim_debug_write_edid,
  978. };
  979. static const struct file_operations sim_mst_hpd_fops = {
  980. .open = simple_open,
  981. .write = dp_sim_debug_write_mst_hpd,
  982. };
  983. static ssize_t dp_sim_debug_write_mst_mode(struct file *file,
  984. const char __user *user_buff, size_t count, loff_t *ppos)
  985. {
  986. struct dp_sim_device *debug = file->private_data;
  987. char buf[SZ_16];
  988. size_t len = 0;
  989. int mst_sideband_mode = 0;
  990. u32 mst_port_cnt = 0;
  991. u32 mst_old_port_cnt;
  992. struct dp_sim_debug_edid_entry *edid_entry;
  993. u8 *edid;
  994. u32 i, rc;
  995. if (!debug)
  996. return -ENODEV;
  997. /* Leave room for termination char */
  998. len = min_t(size_t, count, SZ_8 - 1);
  999. if (copy_from_user(buf, user_buff, len))
  1000. return -EFAULT;
  1001. buf[len] = '\0';
  1002. if (sscanf(buf, "%d %u", &mst_sideband_mode, &mst_port_cnt) != 2) {
  1003. DP_ERR("invalid input\n");
  1004. return -EINVAL;
  1005. }
  1006. if (mst_port_cnt >= MAX_MST_PORT) {
  1007. DP_ERR("port cnt:%d exceeding max:%d\n", mst_port_cnt,
  1008. MAX_MST_PORT);
  1009. return -EINVAL;
  1010. }
  1011. if (!mst_port_cnt)
  1012. mst_port_cnt = 1;
  1013. debug->skip_mst = !mst_sideband_mode;
  1014. DP_DEBUG("mst_sideband_mode: %d port_cnt:%d\n",
  1015. mst_sideband_mode, mst_port_cnt);
  1016. mst_old_port_cnt = debug->port_num;
  1017. rc = dp_sim_update_port_num(&debug->bridge, mst_port_cnt);
  1018. if (rc)
  1019. return rc;
  1020. /* write mst */
  1021. dp_sim_write_dpcd(debug, DP_MSTM_CAP, debug->skip_mst);
  1022. /* create default edid nodes */
  1023. for (i = mst_old_port_cnt; i < mst_port_cnt; i++) {
  1024. edid_entry = devm_kzalloc(debug->dev,
  1025. sizeof(*edid_entry), GFP_KERNEL);
  1026. if (!edid_entry)
  1027. continue;
  1028. edid_entry->index = i;
  1029. edid_entry->sim_dev = debug;
  1030. scnprintf(buf, sizeof(buf), "edid-%d", i);
  1031. debugfs_create_file(buf,
  1032. 0444,
  1033. debug->debugfs_edid_dir,
  1034. edid_entry,
  1035. &sim_edid_fops);
  1036. scnprintf(buf, sizeof(buf), "hpd-%d", i);
  1037. debugfs_create_file(buf,
  1038. 0444,
  1039. debug->debugfs_edid_dir,
  1040. edid_entry,
  1041. &sim_mst_hpd_fops);
  1042. if (!debug->ports[0].edid_size)
  1043. continue;
  1044. edid = devm_kzalloc(debug->dev,
  1045. debug->ports[0].edid_size, GFP_KERNEL);
  1046. if (!edid)
  1047. return -ENOMEM;
  1048. memcpy(edid, debug->ports[0].edid, debug->ports[0].edid_size);
  1049. debug->ports[i].edid = edid;
  1050. debug->ports[i].edid_size = debug->ports[0].edid_size;
  1051. }
  1052. return count;
  1053. }
  1054. static const struct file_operations sim_dpcd_fops = {
  1055. .open = simple_open,
  1056. .write = dp_sim_debug_write_dpcd,
  1057. .read = dp_sim_debug_read_dpcd,
  1058. };
  1059. static const struct file_operations sim_hpd_fops = {
  1060. .open = simple_open,
  1061. .write = dp_sim_debug_write_hpd,
  1062. };
  1063. static const struct file_operations sim_skip_link_training_fops = {
  1064. .open = simple_open,
  1065. .write = dp_sim_debug_write_skip_link_training,
  1066. };
  1067. static const struct file_operations sim_skip_edid_fops = {
  1068. .open = simple_open,
  1069. .write = dp_sim_debug_write_skip_edid,
  1070. };
  1071. static const struct file_operations sim_skip_dpcd_fops = {
  1072. .open = simple_open,
  1073. .write = dp_sim_debug_write_skip_dpcd,
  1074. };
  1075. static const struct file_operations sim_skip_config_fops = {
  1076. .open = simple_open,
  1077. .write = dp_sim_debug_write_skip_config,
  1078. };
  1079. static const struct file_operations sim_mst_mode_fops = {
  1080. .open = simple_open,
  1081. .write = dp_sim_debug_write_mst_mode,
  1082. };
  1083. static int dp_sim_debug_init(struct dp_sim_device *sim_dev)
  1084. {
  1085. struct dp_sim_debug_edid_entry *edid_entry;
  1086. struct dentry *dir, *file, *edid_dir;
  1087. char name[SZ_16];
  1088. int rc = 0, i;
  1089. if (!sim_dev->label)
  1090. return 0;
  1091. dir = debugfs_create_dir(sim_dev->label, NULL);
  1092. if (IS_ERR_OR_NULL(dir)) {
  1093. rc = PTR_ERR(dir);
  1094. DP_ERR("[%s] debugfs create dir failed, rc = %d\n",
  1095. sim_dev->label, rc);
  1096. goto error;
  1097. }
  1098. edid_dir = debugfs_create_dir("mst_edid", dir);
  1099. if (IS_ERR_OR_NULL(edid_dir)) {
  1100. rc = PTR_ERR(edid_dir);
  1101. DP_ERR("[%s] debugfs create dir failed, rc = %d\n",
  1102. sim_dev->label, rc);
  1103. goto error_remove_dir;
  1104. }
  1105. for (i = 0; i < sim_dev->port_num; i++) {
  1106. edid_entry = devm_kzalloc(sim_dev->dev,
  1107. sizeof(*edid_entry), GFP_KERNEL);
  1108. edid_entry->index = i;
  1109. edid_entry->sim_dev = sim_dev;
  1110. scnprintf(name, sizeof(name), "edid-%d", i);
  1111. file = debugfs_create_file(name,
  1112. 0444,
  1113. edid_dir,
  1114. edid_entry,
  1115. &sim_edid_fops);
  1116. if (IS_ERR_OR_NULL(file)) {
  1117. rc = PTR_ERR(file);
  1118. DP_ERR("[%s] debugfs create edid failed, rc=%d\n",
  1119. sim_dev->label, rc);
  1120. goto error_remove_dir;
  1121. }
  1122. scnprintf(name, sizeof(name), "hpd-%d", i);
  1123. file = debugfs_create_file(name,
  1124. 0444,
  1125. edid_dir,
  1126. edid_entry,
  1127. &sim_mst_hpd_fops);
  1128. if (IS_ERR_OR_NULL(file)) {
  1129. rc = PTR_ERR(file);
  1130. DP_ERR("[%s] debugfs create hpd failed, rc=%d\n",
  1131. sim_dev->label, rc);
  1132. goto error_remove_dir;
  1133. }
  1134. }
  1135. file = debugfs_create_symlink("edid", dir, "./mst_edid/edid-0");
  1136. if (IS_ERR_OR_NULL(file)) {
  1137. rc = PTR_ERR(file);
  1138. DP_ERR("[%s] debugfs create edid link failed, rc=%d\n",
  1139. sim_dev->label, rc);
  1140. goto error_remove_dir;
  1141. }
  1142. file = debugfs_create_file("dpcd",
  1143. 0444,
  1144. dir,
  1145. sim_dev,
  1146. &sim_dpcd_fops);
  1147. if (IS_ERR_OR_NULL(file)) {
  1148. rc = PTR_ERR(file);
  1149. DP_ERR("[%s] debugfs create failed, rc=%d\n",
  1150. sim_dev->label, rc);
  1151. goto error_remove_dir;
  1152. }
  1153. file = debugfs_create_file("hpd",
  1154. 0444,
  1155. dir,
  1156. sim_dev,
  1157. &sim_hpd_fops);
  1158. if (IS_ERR_OR_NULL(file)) {
  1159. rc = PTR_ERR(file);
  1160. DP_ERR("[%s] debugfs create failed, rc=%d\n",
  1161. sim_dev->label, rc);
  1162. goto error_remove_dir;
  1163. }
  1164. file = debugfs_create_file("skip_link_training",
  1165. 0444,
  1166. dir,
  1167. sim_dev,
  1168. &sim_skip_link_training_fops);
  1169. if (IS_ERR_OR_NULL(file)) {
  1170. rc = PTR_ERR(file);
  1171. DP_ERR("[%s] debugfs create failed, rc=%d\n",
  1172. sim_dev->label, rc);
  1173. goto error_remove_dir;
  1174. }
  1175. file = debugfs_create_file("skip_edid",
  1176. 0444,
  1177. dir,
  1178. sim_dev,
  1179. &sim_skip_edid_fops);
  1180. if (IS_ERR_OR_NULL(file)) {
  1181. rc = PTR_ERR(file);
  1182. DP_ERR("[%s] debugfs create failed, rc=%d\n",
  1183. sim_dev->label, rc);
  1184. goto error_remove_dir;
  1185. }
  1186. file = debugfs_create_file("skip_dpcd_read",
  1187. 0444,
  1188. dir,
  1189. sim_dev,
  1190. &sim_skip_dpcd_fops);
  1191. if (IS_ERR_OR_NULL(file)) {
  1192. rc = PTR_ERR(file);
  1193. DP_ERR("[%s] debugfs create failed, rc=%d\n",
  1194. sim_dev->label, rc);
  1195. goto error_remove_dir;
  1196. }
  1197. file = debugfs_create_file("skip_dpcd_write",
  1198. 0444,
  1199. dir,
  1200. sim_dev,
  1201. &sim_skip_config_fops);
  1202. if (IS_ERR_OR_NULL(file)) {
  1203. rc = PTR_ERR(file);
  1204. DP_ERR("[%s] debugfs create failed, rc=%d\n",
  1205. sim_dev->label, rc);
  1206. goto error_remove_dir;
  1207. }
  1208. file = debugfs_create_file("mst_sideband_mode",
  1209. 0444,
  1210. dir,
  1211. sim_dev,
  1212. &sim_mst_mode_fops);
  1213. if (IS_ERR_OR_NULL(file)) {
  1214. rc = PTR_ERR(file);
  1215. DP_ERR("[%s] debugfs create failed, rc=%d\n",
  1216. sim_dev->label, rc);
  1217. goto error_remove_dir;
  1218. }
  1219. sim_dev->debugfs_dir = dir;
  1220. sim_dev->debugfs_edid_dir = edid_dir;
  1221. return 0;
  1222. error_remove_dir:
  1223. debugfs_remove_recursive(dir);
  1224. error:
  1225. return rc;
  1226. }
  1227. static int dp_sim_parse(struct dp_sim_device *sim_dev)
  1228. {
  1229. int rc;
  1230. sim_dev->label = of_get_property(sim_dev->bridge.of_node,
  1231. "label", NULL);
  1232. rc = dp_sim_parse_dpcd(sim_dev);
  1233. if (rc) {
  1234. DP_ERR("failed to parse DPCD nodes\n");
  1235. return rc;
  1236. }
  1237. rc = dp_sim_parse_edid(sim_dev);
  1238. if (rc) {
  1239. DP_ERR("failed to parse EDID nodes\n");
  1240. return rc;
  1241. }
  1242. rc = dp_sim_parse_misc(sim_dev);
  1243. if (rc) {
  1244. DP_ERR("failed to parse misc nodes\n");
  1245. return rc;
  1246. }
  1247. return 0;
  1248. }
  1249. int dp_sim_create_bridge(struct device *dev, struct dp_aux_bridge **bridge)
  1250. {
  1251. struct dp_sim_device *dp_sim_dev;
  1252. struct dp_mst_sim_cfg cfg;
  1253. int ret;
  1254. dp_sim_dev = devm_kzalloc(dev, sizeof(*dp_sim_dev), GFP_KERNEL);
  1255. if (!dp_sim_dev)
  1256. return -ENOMEM;
  1257. dp_sim_dev->dev = dev;
  1258. dp_sim_dev->bridge.of_node = dev->of_node;
  1259. dp_sim_dev->bridge.register_hpd = dp_sim_register_hpd;
  1260. dp_sim_dev->bridge.transfer = dp_sim_transfer;
  1261. dp_sim_dev->bridge.dev_priv = dp_sim_dev;
  1262. dp_sim_dev->bridge.flag = DP_AUX_BRIDGE_MST | DP_SIM_BRIDGE_PRIV_FLAG;
  1263. INIT_LIST_HEAD(&dp_sim_dev->dpcd_reg_list);
  1264. mutex_init(&dp_sim_dev->lock);
  1265. memset(&cfg, 0, sizeof(cfg));
  1266. cfg.host_dev = dp_sim_dev;
  1267. cfg.host_hpd_irq = dp_sim_host_hpd_irq;
  1268. ret = dp_mst_sim_create(&cfg, &dp_sim_dev->bridge.mst_ctx);
  1269. if (ret) {
  1270. devm_kfree(dev, dp_sim_dev);
  1271. return ret;
  1272. }
  1273. /* default dpcd reg value */
  1274. dp_sim_dev->dpcd_reg[DP_DPCD_REV] = 0x12;
  1275. dp_sim_dev->dpcd_reg[DP_MAX_LINK_RATE] = 0x14;
  1276. dp_sim_dev->dpcd_reg[DP_MAX_LANE_COUNT] = 0xc4;
  1277. dp_sim_dev->dpcd_reg[DP_SINK_COUNT] = 0x1;
  1278. dp_sim_dev->dpcd_reg[DP_LANE0_1_STATUS] = 0x77;
  1279. dp_sim_dev->dpcd_reg[DP_LANE2_3_STATUS] = 0x77;
  1280. dp_sim_dev->dpcd_reg[DP_LANE_ALIGN_STATUS_UPDATED] = 0x1;
  1281. dp_sim_dev->dpcd_reg[DP_SINK_STATUS] = 0x3;
  1282. dp_sim_dev->dpcd_reg[DP_PAYLOAD_TABLE_UPDATE_STATUS] = 0x3;
  1283. /* enable link training by default */
  1284. dp_sim_dev->skip_link_training = true;
  1285. dp_sim_dev->link_training_cnt = (u32)-1;
  1286. *bridge = &dp_sim_dev->bridge;
  1287. return 0;
  1288. }
  1289. int dp_sim_destroy_bridge(struct dp_aux_bridge *bridge)
  1290. {
  1291. struct dp_sim_device *dp_sim_dev;
  1292. struct dp_sim_dpcd_reg *reg, *p;
  1293. if (!bridge || !(bridge->flag & DP_SIM_BRIDGE_PRIV_FLAG))
  1294. return -EINVAL;
  1295. dp_sim_dev = to_dp_sim_dev(bridge);
  1296. dp_mst_sim_destroy(dp_sim_dev->bridge.mst_ctx);
  1297. list_for_each_entry_safe(reg, p, &dp_sim_dev->dpcd_reg_list, head) {
  1298. list_del(&reg->head);
  1299. devm_kfree(dp_sim_dev->dev, reg);
  1300. }
  1301. if (dp_sim_dev->ports)
  1302. devm_kfree(dp_sim_dev->dev, dp_sim_dev->ports);
  1303. devm_kfree(dp_sim_dev->dev, dp_sim_dev);
  1304. return 0;
  1305. }
  1306. int dp_sim_probe(struct platform_device *pdev)
  1307. {
  1308. struct dp_sim_device *dp_sim_dev;
  1309. struct dp_aux_bridge *bridge;
  1310. int ret;
  1311. ret = dp_sim_create_bridge(&pdev->dev, &bridge);
  1312. if (ret)
  1313. return ret;
  1314. dp_sim_dev = to_dp_sim_dev(bridge);
  1315. ret = dp_sim_parse(dp_sim_dev);
  1316. if (ret)
  1317. goto fail;
  1318. if (dp_sim_dev->skip_hpd)
  1319. dp_sim_dev->bridge.flag |= DP_AUX_BRIDGE_HPD;
  1320. ret = dp_mst_sim_update(dp_sim_dev->bridge.mst_ctx,
  1321. dp_sim_dev->port_num, dp_sim_dev->ports);
  1322. if (ret)
  1323. goto fail;
  1324. ret = dp_sim_debug_init(dp_sim_dev);
  1325. if (ret)
  1326. goto fail;
  1327. ret = dp_aux_add_bridge(&dp_sim_dev->bridge);
  1328. if (ret)
  1329. goto fail;
  1330. platform_set_drvdata(pdev, dp_sim_dev);
  1331. return 0;
  1332. fail:
  1333. dp_sim_destroy_bridge(bridge);
  1334. return ret;
  1335. }
  1336. int dp_sim_remove(struct platform_device *pdev)
  1337. {
  1338. struct dp_sim_device *dp_sim_dev;
  1339. dp_sim_dev = platform_get_drvdata(pdev);
  1340. if (!dp_sim_dev)
  1341. return 0;
  1342. debugfs_remove_recursive(dp_sim_dev->debugfs_dir);
  1343. dp_sim_destroy_bridge(&dp_sim_dev->bridge);
  1344. return 0;
  1345. }