ssp_sensorhub.c 19 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714
  1. /*
  2. * Copyright (C) 2013, Samsung Electronics Co. Ltd. All Rights Reserved.
  3. *
  4. * This program is free software; you can redistribute it and/or modify
  5. * it under the terms of the GNU General Public License as published by
  6. * the Free Software Foundation; either version 2 of the License, or
  7. * (at your option) any later version.
  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 "ssp_sensorhub.h"
  16. static void ssp_sensorhub_log(const char *func_name,
  17. const char *data, int length)
  18. {
  19. char buf[6];
  20. char *log_str;
  21. int log_size;
  22. int i;
  23. if (likely(length <= BIG_DATA_SIZE))
  24. log_size = length;
  25. else
  26. log_size = PRINT_TRUNCATE * 2 + 1;
  27. log_size = sizeof(buf) * log_size + 1;
  28. log_str = kzalloc(log_size, GFP_ATOMIC);
  29. if (unlikely(!log_str)) {
  30. sensorhub_err("allocate memory for data log err");
  31. return;
  32. }
  33. for (i = 0; i < length; i++) {
  34. if (length < BIG_DATA_SIZE ||
  35. i < PRINT_TRUNCATE || i >= length - PRINT_TRUNCATE) {
  36. snprintf(buf, sizeof(buf), "%d", (signed char)data[i]);
  37. strlcat(log_str, buf, log_size);
  38. }
  39. if (length < BIG_DATA_SIZE ||
  40. i < PRINT_TRUNCATE || i >= length - PRINT_TRUNCATE) {
  41. if (i < length - 1)
  42. strlcat(log_str, ", ", log_size);
  43. }
  44. if (length > BIG_DATA_SIZE && i == PRINT_TRUNCATE)
  45. strlcat(log_str, "..., ", log_size);
  46. }
  47. pr_info("[SSP]: %s - %s (%d)", func_name, log_str, length);
  48. kfree(log_str);
  49. }
  50. static int ssp_sensorhub_send_big_data(struct ssp_sensorhub_data *hub_data,
  51. const char *buf, int count)
  52. {
  53. int length = count - 3;
  54. int ret = 0;
  55. /* only support voice service for the moment */
  56. if (buf[1] != TYPE_WAKE_UP_VOICE_SERVICE) {
  57. sensorhub_err("not voice service(%d)", buf[1]);
  58. return -EINVAL;
  59. }
  60. /* am or grammer data? */
  61. if (buf[2] != TYPE_WAKE_UP_VOICE_SOUND_SOURCE_AM &&
  62. buf[2] != TYPE_WAKE_UP_VOICE_SOUND_SOURCE_GRAMMER) {
  63. sensorhub_err("voice data type err(%d)", buf[2]);
  64. return -EINVAL;
  65. }
  66. hub_data->big_send_events.library_data
  67. = kzalloc(length * sizeof(char), GFP_KERNEL);
  68. if (unlikely(!hub_data->big_send_events.library_data)) {
  69. sensorhub_err("allocate memory for big library err");
  70. return -ENOMEM;
  71. }
  72. memcpy(hub_data->big_send_events.library_data, buf+3, length);
  73. hub_data->big_send_events.library_length = length;
  74. /* trigger big data transfer */
  75. ret = set_big_data_start(hub_data->ssp_data, buf[2]+1, length);
  76. if (ret != SUCCESS) {
  77. sensorhub_err("set_big_data_start err(%d)", ret);
  78. goto exit;
  79. }
  80. /* wait until write operation is done */
  81. ret = wait_for_completion_timeout(&hub_data->big_write_done,
  82. COMPLETION_TIMEOUT + 1*HZ);
  83. if (unlikely(!ret)) {
  84. sensorhub_err("wait timed out");
  85. ret = -EBUSY;
  86. } else if (unlikely(ret < 0)) {
  87. sensorhub_err("wait for completion err(%d)", ret);
  88. ret = -EIO;
  89. }
  90. exit:
  91. kfree(hub_data->big_send_events.library_data);
  92. return !ret ? ret + 1 : ret;
  93. }
  94. static int ssp_sensorhub_send_cmd(struct ssp_sensorhub_data *hub_data,
  95. const char *buf, int count)
  96. {
  97. int ret = 0;
  98. if (buf[2] < MSG2SSP_AP_STATUS_WAKEUP ||
  99. buf[2] >= MSG2SSP_AP_TEMPHUMIDITY_CAL_DONE) {
  100. sensorhub_err("MSG2SSP_INST_LIB_NOTI err(%d)", buf[2]);
  101. return -EINVAL;
  102. }
  103. ret = ssp_send_cmd(hub_data->ssp_data, buf[2], 0);
  104. if (buf[2] == MSG2SSP_AP_STATUS_WAKEUP ||
  105. buf[2] == MSG2SSP_AP_STATUS_SLEEP)
  106. hub_data->ssp_data->uLastAPState = buf[2];
  107. if (buf[2] == MSG2SSP_AP_STATUS_SUSPEND ||
  108. buf[2] == MSG2SSP_AP_STATUS_RESUME)
  109. hub_data->ssp_data->uLastResumeState = buf[2];
  110. return ret;
  111. }
  112. static int ssp_sensorhub_send_instruction(struct ssp_sensorhub_data *hub_data,
  113. const char *buf, int count)
  114. {
  115. unsigned char instruction = buf[0];
  116. if (buf[0] == MSG2SSP_INST_LIBRARY_REMOVE)
  117. instruction = REMOVE_LIBRARY;
  118. else if (buf[0] == MSG2SSP_INST_LIBRARY_ADD)
  119. instruction = ADD_LIBRARY;
  120. return send_instruction(hub_data->ssp_data, instruction,
  121. (unsigned char)buf[1], (unsigned char *)(buf+2), count-2);
  122. }
  123. static ssize_t ssp_sensorhub_write(struct file *file, const char __user *buf,
  124. size_t count, loff_t *pos)
  125. {
  126. struct ssp_sensorhub_data *hub_data
  127. = container_of(file->private_data,
  128. struct ssp_sensorhub_data, sensorhub_device);
  129. int ret = 0;
  130. if (unlikely(count < 2)) {
  131. sensorhub_err("library data length err(%d)", count);
  132. return -EINVAL;
  133. }
  134. ssp_sensorhub_log(__func__, buf, count);
  135. if (unlikely(hub_data->ssp_data->bSspShutdown)) {
  136. sensorhub_err("stop sending library data(shutdown)");
  137. return -EBUSY;
  138. }
  139. if (buf[0] == MSG2SSP_INST_LIB_DATA && count >= BIG_DATA_SIZE)
  140. ret = ssp_sensorhub_send_big_data(hub_data, buf, count);
  141. else if (buf[0] == MSG2SSP_INST_LIB_NOTI)
  142. ret = ssp_sensorhub_send_cmd(hub_data, buf, count);
  143. else
  144. ret = ssp_sensorhub_send_instruction(hub_data, buf, count);
  145. if (unlikely(ret <= 0)) {
  146. sensorhub_err("send library data err(%d)", ret);
  147. /* i2c transfer fail */
  148. if (ret == ERROR)
  149. return -EIO;
  150. /* i2c transfer done but no ack from MCU */
  151. else if (ret == FAIL)
  152. return -EAGAIN;
  153. else
  154. return ret;
  155. }
  156. return count;
  157. }
  158. static ssize_t ssp_sensorhub_read(struct file *file, char __user *buf,
  159. size_t count, loff_t *pos)
  160. {
  161. struct ssp_sensorhub_data *hub_data
  162. = container_of(file->private_data,
  163. struct ssp_sensorhub_data, sensorhub_device);
  164. struct sensorhub_event *event;
  165. int retries = MAX_DATA_COPY_TRY;
  166. int length = 0;
  167. int ret = 0;
  168. spin_lock_bh(&hub_data->sensorhub_lock);
  169. if (unlikely(kfifo_is_empty(&hub_data->fifo))) {
  170. sensorhub_info("no library data");
  171. goto err;
  172. }
  173. /* first in first out */
  174. ret = kfifo_out_peek(&hub_data->fifo, &event, sizeof(void *));
  175. if (unlikely(!ret)) {
  176. sensorhub_err("kfifo out peek err(%d)", ret);
  177. ret = EIO;
  178. goto err;
  179. }
  180. length = event->library_length;
  181. while (retries--) {
  182. ret = copy_to_user(buf,
  183. event->library_data, event->library_length);
  184. if (likely(!ret))
  185. break;
  186. }
  187. if (unlikely(ret)) {
  188. sensorhub_err("read library data err(%d)", ret);
  189. goto err;
  190. }
  191. ssp_sensorhub_log(__func__,
  192. event->library_data, event->library_length);
  193. /* remove first event from the list */
  194. ret = kfifo_out(&hub_data->fifo, &event, sizeof(void *));
  195. if (unlikely(ret != sizeof(void *))) {
  196. sensorhub_err("kfifo out err(%d)", ret);
  197. ret = EIO;
  198. goto err;
  199. }
  200. complete(&hub_data->read_done);
  201. spin_unlock_bh(&hub_data->sensorhub_lock);
  202. return length;
  203. err:
  204. spin_unlock_bh(&hub_data->sensorhub_lock);
  205. return ret ? -ret : 0;
  206. }
  207. static long ssp_sensorhub_ioctl(struct file *file, unsigned int cmd,
  208. unsigned long arg)
  209. {
  210. struct ssp_sensorhub_data *hub_data
  211. = container_of(file->private_data,
  212. struct ssp_sensorhub_data, sensorhub_device);
  213. void __user *argp = (void __user *)arg;
  214. int retries = MAX_DATA_COPY_TRY;
  215. int length = hub_data->big_events.library_length;
  216. int ret = 0;
  217. switch (cmd) {
  218. case IOCTL_READ_BIG_CONTEXT_DATA:
  219. if (unlikely(!hub_data->big_events.library_data
  220. || !hub_data->big_events.library_length)) {
  221. sensorhub_info("no big library data");
  222. return 0;
  223. }
  224. while (retries--) {
  225. ret = copy_to_user(argp,
  226. hub_data->big_events.library_data,
  227. hub_data->big_events.library_length);
  228. if (likely(!ret))
  229. break;
  230. }
  231. if (unlikely(ret)) {
  232. sensorhub_err("read big library data err(%d)", ret);
  233. return -ret;
  234. }
  235. ssp_sensorhub_log(__func__,
  236. hub_data->big_events.library_data,
  237. hub_data->big_events.library_length);
  238. hub_data->is_big_event = false;
  239. kfree(hub_data->big_events.library_data);
  240. hub_data->big_events.library_length = 0;
  241. complete(&hub_data->big_read_done);
  242. break;
  243. default:
  244. sensorhub_err("ioctl cmd err(%d)", cmd);
  245. return -EINVAL;
  246. }
  247. return length;
  248. }
  249. static struct file_operations ssp_sensorhub_fops = {
  250. .owner = THIS_MODULE,
  251. .open = nonseekable_open,
  252. .write = ssp_sensorhub_write,
  253. .read = ssp_sensorhub_read,
  254. .unlocked_ioctl = ssp_sensorhub_ioctl,
  255. };
  256. void ssp_sensorhub_report_notice(struct ssp_data *ssp_data, char notice)
  257. {
  258. struct ssp_sensorhub_data *hub_data = ssp_data->hub_data;
  259. input_report_rel(hub_data->sensorhub_input_dev, NOTICE, notice);
  260. input_sync(hub_data->sensorhub_input_dev);
  261. if (notice == MSG2SSP_AP_STATUS_WAKEUP)
  262. sensorhub_info("wake up");
  263. else if (notice == MSG2SSP_AP_STATUS_SLEEP)
  264. sensorhub_info("sleep");
  265. else if (notice == MSG2SSP_AP_STATUS_RESET)
  266. sensorhub_info("reset");
  267. else
  268. sensorhub_err("invalid notice(0x%x)", notice);
  269. }
  270. static void ssp_sensorhub_report_library(struct ssp_sensorhub_data *hub_data)
  271. {
  272. input_report_rel(hub_data->sensorhub_input_dev, DATA, DATA);
  273. input_sync(hub_data->sensorhub_input_dev);
  274. wake_lock_timeout(&hub_data->sensorhub_wake_lock, WAKE_LOCK_TIMEOUT);
  275. }
  276. static void ssp_sensorhub_report_big_library(
  277. struct ssp_sensorhub_data *hub_data)
  278. {
  279. input_report_rel(hub_data->sensorhub_input_dev, BIG_DATA, BIG_DATA);
  280. input_sync(hub_data->sensorhub_input_dev);
  281. wake_lock_timeout(&hub_data->sensorhub_wake_lock, WAKE_LOCK_TIMEOUT);
  282. }
  283. static int ssp_sensorhub_list(struct ssp_sensorhub_data *hub_data,
  284. char *dataframe, int length)
  285. {
  286. struct sensorhub_event *event;
  287. int ret = 0;
  288. if (unlikely(length <= 0 || length >= PAGE_SIZE)) {
  289. sensorhub_err("library length err(%d)", length);
  290. return -EINVAL;
  291. }
  292. ssp_sensorhub_log(__func__, dataframe, length);
  293. /* overwrite new event if list is full */
  294. if (unlikely(kfifo_is_full(&hub_data->fifo))) {
  295. ret = kfifo_out(&hub_data->fifo, &event, sizeof(void *));
  296. if (unlikely(ret != sizeof(void *))) {
  297. sensorhub_err("kfifo out err(%d)", ret);
  298. return -EIO;
  299. }
  300. sensorhub_info("overwrite event");
  301. }
  302. /* allocate memory for new event */
  303. kfree(hub_data->events[hub_data->event_number].library_data);
  304. hub_data->events[hub_data->event_number].library_data
  305. = kzalloc(length * sizeof(char), GFP_ATOMIC);
  306. if (unlikely(!hub_data->events[hub_data->event_number].library_data)) {
  307. sensorhub_err("allocate memory for library err");
  308. return -ENOMEM;
  309. }
  310. /* copy new event into memory */
  311. memcpy(hub_data->events[hub_data->event_number].library_data,
  312. dataframe, length);
  313. hub_data->events[hub_data->event_number].library_length = length;
  314. /* add new event into the end of list */
  315. event = &hub_data->events[hub_data->event_number];
  316. ret = kfifo_in(&hub_data->fifo, &event, sizeof(void *));
  317. if (unlikely(ret != sizeof(void *))) {
  318. sensorhub_err("kfifo in err(%d)", ret);
  319. return -EIO;
  320. }
  321. /* not to overflow max list capacity */
  322. if (hub_data->event_number++ >= LIST_SIZE - 1)
  323. hub_data->event_number = 0;
  324. return kfifo_len(&hub_data->fifo) / sizeof(void *);
  325. }
  326. int ssp_sensorhub_handle_data(struct ssp_data *ssp_data, char *dataframe,
  327. int start, int end)
  328. {
  329. struct ssp_sensorhub_data *hub_data = ssp_data->hub_data;
  330. int ret = 0;
  331. /* add new sensorhub event into list */
  332. spin_lock_bh(&hub_data->sensorhub_lock);
  333. ret = ssp_sensorhub_list(hub_data, dataframe+start, end-start);
  334. spin_unlock_bh(&hub_data->sensorhub_lock);
  335. if (ret < 0)
  336. sensorhub_err("sensorhub list err(%d)", ret);
  337. else
  338. wake_up(&hub_data->sensorhub_wq);
  339. return ret;
  340. }
  341. static int ssp_sensorhub_thread(void *arg)
  342. {
  343. struct ssp_sensorhub_data *hub_data = (struct ssp_sensorhub_data *)arg;
  344. int ret = 0;
  345. while (likely(!kthread_should_stop())) {
  346. /* run thread if list is not empty */
  347. wait_event_interruptible(hub_data->sensorhub_wq,
  348. kthread_should_stop() ||
  349. !kfifo_is_empty(&hub_data->fifo) ||
  350. hub_data->is_big_event);
  351. /* exit thread if kthread should stop */
  352. if (unlikely(kthread_should_stop())) {
  353. sensorhub_info("kthread_stop()");
  354. break;
  355. }
  356. if (likely(!kfifo_is_empty(&hub_data->fifo))) {
  357. /* report sensorhub event to user */
  358. ssp_sensorhub_report_library(hub_data);
  359. /* wait until transfer finished */
  360. ret = wait_for_completion_timeout(
  361. &hub_data->read_done, COMPLETION_TIMEOUT);
  362. if (unlikely(!ret))
  363. sensorhub_err("wait for read timed out");
  364. else if (unlikely(ret < 0))
  365. sensorhub_err("read completion err(%d)", ret);
  366. }
  367. if (unlikely(hub_data->is_big_event)) {
  368. /* report big sensorhub event to user */
  369. ssp_sensorhub_report_big_library(hub_data);
  370. /* wait until transfer finished */
  371. ret = wait_for_completion_timeout(
  372. &hub_data->big_read_done, COMPLETION_TIMEOUT);
  373. if (unlikely(!ret))
  374. sensorhub_err("wait for big read timed out");
  375. else if (unlikely(ret < 0))
  376. sensorhub_err("big read completion err(%d)",
  377. ret);
  378. }
  379. }
  380. return 0;
  381. }
  382. void ssp_read_big_library_task(struct work_struct *work)
  383. {
  384. struct ssp_big *big = container_of(work, struct ssp_big, work);
  385. struct ssp_sensorhub_data *hub_data = big->data->hub_data;
  386. struct ssp_msg *msg;
  387. int buf_len, residue = big->length, ret = 0, index = 0, pos = 0;
  388. hub_data->big_events.library_length = big->length;
  389. hub_data->big_events.library_data = kzalloc(big->length, GFP_KERNEL);
  390. while (residue > 0) {
  391. buf_len = residue > DATA_PACKET_SIZE
  392. ? DATA_PACKET_SIZE : residue;
  393. msg = kzalloc(sizeof(*msg), GFP_KERNEL);
  394. msg->cmd = MSG2SSP_AP_GET_BIG_DATA;
  395. msg->length = buf_len;
  396. msg->options = AP2HUB_READ | (index++ << SSP_INDEX);
  397. msg->data = big->addr;
  398. msg->buffer = hub_data->big_events.library_data + pos;
  399. msg->free_buffer = 0;
  400. ret = ssp_spi_sync(big->data, msg, 1000);
  401. if (ret != SUCCESS) {
  402. sensorhub_err("read big data err(%d)", ret);
  403. break;
  404. }
  405. pos += buf_len;
  406. residue -= buf_len;
  407. sensorhub_info("read big data (%5d / %5d)", pos, big->length);
  408. }
  409. hub_data->is_big_event = true;
  410. wake_up(&hub_data->sensorhub_wq);
  411. kfree(big);
  412. }
  413. void ssp_send_big_library_task(struct work_struct *work)
  414. {
  415. struct ssp_big *big = container_of(work, struct ssp_big, work);
  416. struct ssp_sensorhub_data *hub_data = big->data->hub_data;
  417. struct ssp_msg *msg;
  418. int buf_len, residue = big->length, ret = 0, index = 0, pos = 0;
  419. while (residue > 0) {
  420. buf_len = residue > DATA_PACKET_SIZE
  421. ? DATA_PACKET_SIZE : residue;
  422. msg = kzalloc(sizeof(*msg), GFP_KERNEL);
  423. msg->cmd = MSG2SSP_AP_SET_BIG_DATA;
  424. msg->length = buf_len;
  425. msg->options = AP2HUB_WRITE | (index++ << SSP_INDEX);
  426. msg->data = big->addr;
  427. msg->buffer = hub_data->big_send_events.library_data + pos;
  428. msg->free_buffer = 0;
  429. ret = ssp_spi_sync(big->data, msg, 1000);
  430. if (ret != SUCCESS) {
  431. sensorhub_err("send big data err(%d)", ret);
  432. break;
  433. }
  434. pos += buf_len;
  435. residue -= buf_len;
  436. sensorhub_info("send big data (%5d / %5d)", pos, big->length);
  437. }
  438. complete(&hub_data->big_write_done);
  439. kfree(big);
  440. }
  441. void ssp_pcm_dump_task(struct work_struct *work)
  442. {
  443. struct ssp_big *big = container_of(work, struct ssp_big, work);
  444. struct ssp_sensorhub_data *hub_data = big->data->hub_data;
  445. struct ssp_msg *msg;
  446. int buf_len, residue = big->length, ret = 0, index = 0;
  447. mm_segment_t old_fs;
  448. struct file *voice_filp;
  449. char pcm_path[BIN_PATH_SIZE+1];
  450. char *buff;
  451. old_fs = get_fs();
  452. set_fs(KERNEL_DS);
  453. snprintf(pcm_path, BIN_PATH_SIZE,
  454. "/data/voice%d.pcm", hub_data->pcm_cnt);
  455. voice_filp = filp_open(pcm_path, O_RDWR | O_CREAT | O_APPEND, 0666);
  456. if (IS_ERR(voice_filp)) {
  457. sensorhub_err("open pcm dump file err");
  458. goto exit;
  459. }
  460. buf_len = big->length > DATA_PACKET_SIZE
  461. ? DATA_PACKET_SIZE : big->length;
  462. buff = kzalloc(buf_len, GFP_KERNEL);
  463. while (residue > 0) {
  464. buf_len = residue > DATA_PACKET_SIZE
  465. ? DATA_PACKET_SIZE : residue;
  466. msg = kzalloc(sizeof(*msg), GFP_KERNEL);
  467. msg->cmd = MSG2SSP_AP_GET_BIG_DATA;
  468. msg->length = buf_len;
  469. msg->options = AP2HUB_READ | (index++ << SSP_INDEX);
  470. msg->data = big->addr;
  471. msg->buffer = buff;
  472. msg->free_buffer = 0;
  473. ret = ssp_spi_sync(big->data, msg, 1000);
  474. if (ret != SUCCESS) {
  475. sensorhub_err("receive pcm dump err(%d)", ret);
  476. break;
  477. }
  478. ret = voice_filp->f_op->write(voice_filp, (char __user *)buff,
  479. buf_len, &voice_filp->f_pos);
  480. if (ret < 0) {
  481. sensorhub_err("write pcm dump to file err(%d)", ret);
  482. break;
  483. }
  484. residue -= buf_len;
  485. sensorhub_info("write pcm dump...");
  486. }
  487. filp_close(voice_filp, current->files);
  488. kfree(buff);
  489. exit:
  490. set_fs(old_fs);
  491. kfree(big);
  492. }
  493. int ssp_sensorhub_pcm_dump(struct ssp_sensorhub_data *hub_data)
  494. {
  495. hub_data->pcm_cnt++;
  496. return set_big_data_start(hub_data->ssp_data, BIG_TYPE_VOICE_PCM, 0);
  497. }
  498. int ssp_sensorhub_initialize(struct ssp_data *ssp_data)
  499. {
  500. struct ssp_sensorhub_data *hub_data;
  501. int ret;
  502. /* allocate memory for sensorhub data */
  503. hub_data = kzalloc(sizeof(*hub_data), GFP_KERNEL);
  504. if (!hub_data) {
  505. sensorhub_err("allocate memory for sensorhub data err");
  506. ret = -ENOMEM;
  507. goto exit;
  508. }
  509. hub_data->ssp_data = ssp_data;
  510. ssp_data->hub_data = hub_data;
  511. /* init wakelock, list, waitqueue, completion and spinlock */
  512. wake_lock_init(&hub_data->sensorhub_wake_lock, WAKE_LOCK_SUSPEND,
  513. "ssp_sensorhub_wake_lock");
  514. init_waitqueue_head(&hub_data->sensorhub_wq);
  515. init_completion(&hub_data->read_done);
  516. init_completion(&hub_data->big_read_done);
  517. init_completion(&hub_data->big_write_done);
  518. spin_lock_init(&hub_data->sensorhub_lock);
  519. /* allocate sensorhub input device */
  520. hub_data->sensorhub_input_dev = input_allocate_device();
  521. if (!hub_data->sensorhub_input_dev) {
  522. sensorhub_err("allocate sensorhub input device err");
  523. ret = -ENOMEM;
  524. goto err_input_allocate_device_sensorhub;
  525. }
  526. /* set sensorhub input device */
  527. input_set_drvdata(hub_data->sensorhub_input_dev, hub_data);
  528. hub_data->sensorhub_input_dev->name = "ssp_context";
  529. input_set_capability(hub_data->sensorhub_input_dev, EV_REL, DATA);
  530. input_set_capability(hub_data->sensorhub_input_dev, EV_REL, BIG_DATA);
  531. input_set_capability(hub_data->sensorhub_input_dev, EV_REL, NOTICE);
  532. /* register sensorhub input device */
  533. ret = input_register_device(hub_data->sensorhub_input_dev);
  534. if (ret < 0) {
  535. sensorhub_err("register sensorhub input device err(%d)", ret);
  536. input_free_device(hub_data->sensorhub_input_dev);
  537. goto err_input_register_device_sensorhub;
  538. }
  539. /* register sensorhub misc device */
  540. hub_data->sensorhub_device.minor = MISC_DYNAMIC_MINOR;
  541. hub_data->sensorhub_device.name = "ssp_sensorhub";
  542. hub_data->sensorhub_device.fops = &ssp_sensorhub_fops;
  543. ret = misc_register(&hub_data->sensorhub_device);
  544. if (ret < 0) {
  545. sensorhub_err("register sensorhub misc device err(%d)", ret);
  546. goto err_misc_register;
  547. }
  548. /* allocate fifo */
  549. ret = kfifo_alloc(&hub_data->fifo,
  550. sizeof(void *) * LIST_SIZE, GFP_KERNEL);
  551. if (ret) {
  552. sensorhub_err("kfifo allocate err(%d)", ret);
  553. goto err_kfifo_alloc;
  554. }
  555. /* create and run sensorhub thread */
  556. hub_data->sensorhub_task = kthread_run(ssp_sensorhub_thread,
  557. (void *)hub_data, "ssp_sensorhub_thread");
  558. if (IS_ERR(hub_data->sensorhub_task)) {
  559. ret = PTR_ERR(hub_data->sensorhub_task);
  560. goto err_kthread_run;
  561. }
  562. return 0;
  563. err_kthread_run:
  564. kfifo_free(&hub_data->fifo);
  565. err_kfifo_alloc:
  566. misc_deregister(&hub_data->sensorhub_device);
  567. err_misc_register:
  568. input_unregister_device(hub_data->sensorhub_input_dev);
  569. err_input_register_device_sensorhub:
  570. err_input_allocate_device_sensorhub:
  571. complete_all(&hub_data->big_write_done);
  572. complete_all(&hub_data->big_read_done);
  573. complete_all(&hub_data->read_done);
  574. wake_lock_destroy(&hub_data->sensorhub_wake_lock);
  575. kfree(hub_data);
  576. exit:
  577. return ret;
  578. }
  579. void ssp_sensorhub_remove(struct ssp_data *ssp_data)
  580. {
  581. struct ssp_sensorhub_data *hub_data = ssp_data->hub_data;
  582. ssp_sensorhub_fops.write = NULL;
  583. ssp_sensorhub_fops.read = NULL;
  584. ssp_sensorhub_fops.unlocked_ioctl = NULL;
  585. kthread_stop(hub_data->sensorhub_task);
  586. kfifo_free(&hub_data->fifo);
  587. misc_deregister(&hub_data->sensorhub_device);
  588. input_unregister_device(hub_data->sensorhub_input_dev);
  589. complete_all(&hub_data->big_write_done);
  590. complete_all(&hub_data->big_read_done);
  591. complete_all(&hub_data->read_done);
  592. wake_lock_destroy(&hub_data->sensorhub_wake_lock);
  593. kfree(hub_data);
  594. }
  595. MODULE_DESCRIPTION("Seamless Sensor Platform(SSP) sensorhub driver");
  596. MODULE_AUTHOR("Samsung Electronics");
  597. MODULE_LICENSE("GPL");