mspro_block.c 38 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429
  1. /*
  2. * Sony MemoryStick Pro storage support
  3. *
  4. * Copyright (C) 2007 Alex Dubov <oakad@yahoo.com>
  5. *
  6. * This program is free software; you can redistribute it and/or modify
  7. * it under the terms of the GNU General Public License version 2 as
  8. * published by the Free Software Foundation.
  9. *
  10. * Special thanks to Carlos Corbacho for providing various MemoryStick cards
  11. * that made this driver possible.
  12. *
  13. */
  14. #include <linux/blkdev.h>
  15. #include <linux/idr.h>
  16. #include <linux/hdreg.h>
  17. #include <linux/kthread.h>
  18. #include <linux/delay.h>
  19. #include <linux/memstick.h>
  20. #define DRIVER_NAME "mspro_block"
  21. static int major;
  22. module_param(major, int, 0644);
  23. #define MSPRO_BLOCK_MAX_SEGS 32
  24. #define MSPRO_BLOCK_MAX_PAGES ((2 << 16) - 1)
  25. #define MSPRO_BLOCK_SIGNATURE 0xa5c3
  26. #define MSPRO_BLOCK_MAX_ATTRIBUTES 41
  27. enum {
  28. MSPRO_BLOCK_ID_SYSINFO = 0x10,
  29. MSPRO_BLOCK_ID_MODELNAME = 0x15,
  30. MSPRO_BLOCK_ID_MBR = 0x20,
  31. MSPRO_BLOCK_ID_PBR16 = 0x21,
  32. MSPRO_BLOCK_ID_PBR32 = 0x22,
  33. MSPRO_BLOCK_ID_SPECFILEVALUES1 = 0x25,
  34. MSPRO_BLOCK_ID_SPECFILEVALUES2 = 0x26,
  35. MSPRO_BLOCK_ID_DEVINFO = 0x30
  36. };
  37. struct mspro_sys_attr {
  38. size_t size;
  39. void *data;
  40. unsigned char id;
  41. char name[32];
  42. struct device_attribute dev_attr;
  43. };
  44. struct mspro_attr_entry {
  45. unsigned int address;
  46. unsigned int size;
  47. unsigned char id;
  48. unsigned char reserved[3];
  49. } __attribute__((packed));
  50. struct mspro_attribute {
  51. unsigned short signature;
  52. unsigned short version;
  53. unsigned char count;
  54. unsigned char reserved[11];
  55. struct mspro_attr_entry entries[];
  56. } __attribute__((packed));
  57. struct mspro_sys_info {
  58. unsigned char class;
  59. unsigned char reserved0;
  60. unsigned short block_size;
  61. unsigned short block_count;
  62. unsigned short user_block_count;
  63. unsigned short page_size;
  64. unsigned char reserved1[2];
  65. unsigned char assembly_date[8];
  66. unsigned int serial_number;
  67. unsigned char assembly_maker_code;
  68. unsigned char assembly_model_code[3];
  69. unsigned short memory_maker_code;
  70. unsigned short memory_model_code;
  71. unsigned char reserved2[4];
  72. unsigned char vcc;
  73. unsigned char vpp;
  74. unsigned short controller_number;
  75. unsigned short controller_function;
  76. unsigned short start_sector;
  77. unsigned short unit_size;
  78. unsigned char ms_sub_class;
  79. unsigned char reserved3[4];
  80. unsigned char interface_type;
  81. unsigned short controller_code;
  82. unsigned char format_type;
  83. unsigned char reserved4;
  84. unsigned char device_type;
  85. unsigned char reserved5[7];
  86. unsigned char mspro_id[16];
  87. unsigned char reserved6[16];
  88. } __attribute__((packed));
  89. struct mspro_mbr {
  90. unsigned char boot_partition;
  91. unsigned char start_head;
  92. unsigned char start_sector;
  93. unsigned char start_cylinder;
  94. unsigned char partition_type;
  95. unsigned char end_head;
  96. unsigned char end_sector;
  97. unsigned char end_cylinder;
  98. unsigned int start_sectors;
  99. unsigned int sectors_per_partition;
  100. } __attribute__((packed));
  101. struct mspro_specfile {
  102. char name[8];
  103. char ext[3];
  104. unsigned char attr;
  105. unsigned char reserved[10];
  106. unsigned short time;
  107. unsigned short date;
  108. unsigned short cluster;
  109. unsigned int size;
  110. } __attribute__((packed));
  111. struct mspro_devinfo {
  112. unsigned short cylinders;
  113. unsigned short heads;
  114. unsigned short bytes_per_track;
  115. unsigned short bytes_per_sector;
  116. unsigned short sectors_per_track;
  117. unsigned char reserved[6];
  118. } __attribute__((packed));
  119. struct mspro_block_data {
  120. struct memstick_dev *card;
  121. unsigned int usage_count;
  122. struct gendisk *disk;
  123. struct request_queue *queue;
  124. spinlock_t q_lock;
  125. wait_queue_head_t q_wait;
  126. struct task_struct *q_thread;
  127. unsigned short page_size;
  128. unsigned short cylinders;
  129. unsigned short heads;
  130. unsigned short sectors_per_track;
  131. unsigned char system;
  132. unsigned char read_only:1,
  133. active:1,
  134. has_request:1,
  135. data_dir:1;
  136. unsigned char transfer_cmd;
  137. int (*mrq_handler)(struct memstick_dev *card,
  138. struct memstick_request **mrq);
  139. struct attribute_group attr_group;
  140. struct scatterlist req_sg[MSPRO_BLOCK_MAX_SEGS];
  141. unsigned int seg_count;
  142. unsigned int current_seg;
  143. unsigned short current_page;
  144. };
  145. static DEFINE_IDR(mspro_block_disk_idr);
  146. static DEFINE_MUTEX(mspro_block_disk_lock);
  147. /*** Block device ***/
  148. static int mspro_block_bd_open(struct inode *inode, struct file *filp)
  149. {
  150. struct gendisk *disk = inode->i_bdev->bd_disk;
  151. struct mspro_block_data *msb = disk->private_data;
  152. int rc = -ENXIO;
  153. mutex_lock(&mspro_block_disk_lock);
  154. if (msb && msb->card) {
  155. msb->usage_count++;
  156. if ((filp->f_mode & FMODE_WRITE) && msb->read_only)
  157. rc = -EROFS;
  158. else
  159. rc = 0;
  160. }
  161. mutex_unlock(&mspro_block_disk_lock);
  162. return rc;
  163. }
  164. static int mspro_block_disk_release(struct gendisk *disk)
  165. {
  166. struct mspro_block_data *msb = disk->private_data;
  167. int disk_id = disk->first_minor >> MEMSTICK_PART_SHIFT;
  168. mutex_lock(&mspro_block_disk_lock);
  169. if (msb->usage_count) {
  170. msb->usage_count--;
  171. if (!msb->usage_count) {
  172. kfree(msb);
  173. disk->private_data = NULL;
  174. idr_remove(&mspro_block_disk_idr, disk_id);
  175. put_disk(disk);
  176. }
  177. }
  178. mutex_unlock(&mspro_block_disk_lock);
  179. return 0;
  180. }
  181. static int mspro_block_bd_release(struct inode *inode, struct file *filp)
  182. {
  183. struct gendisk *disk = inode->i_bdev->bd_disk;
  184. return mspro_block_disk_release(disk);
  185. }
  186. static int mspro_block_bd_getgeo(struct block_device *bdev,
  187. struct hd_geometry *geo)
  188. {
  189. struct mspro_block_data *msb = bdev->bd_disk->private_data;
  190. geo->heads = msb->heads;
  191. geo->sectors = msb->sectors_per_track;
  192. geo->cylinders = msb->cylinders;
  193. return 0;
  194. }
  195. static struct block_device_operations ms_block_bdops = {
  196. .open = mspro_block_bd_open,
  197. .release = mspro_block_bd_release,
  198. .getgeo = mspro_block_bd_getgeo,
  199. .owner = THIS_MODULE
  200. };
  201. /*** Information ***/
  202. static struct mspro_sys_attr *mspro_from_sysfs_attr(struct attribute *attr)
  203. {
  204. struct device_attribute *dev_attr
  205. = container_of(attr, struct device_attribute, attr);
  206. return container_of(dev_attr, struct mspro_sys_attr, dev_attr);
  207. }
  208. static const char *mspro_block_attr_name(unsigned char tag)
  209. {
  210. switch (tag) {
  211. case MSPRO_BLOCK_ID_SYSINFO:
  212. return "attr_sysinfo";
  213. case MSPRO_BLOCK_ID_MODELNAME:
  214. return "attr_modelname";
  215. case MSPRO_BLOCK_ID_MBR:
  216. return "attr_mbr";
  217. case MSPRO_BLOCK_ID_PBR16:
  218. return "attr_pbr16";
  219. case MSPRO_BLOCK_ID_PBR32:
  220. return "attr_pbr32";
  221. case MSPRO_BLOCK_ID_SPECFILEVALUES1:
  222. return "attr_specfilevalues1";
  223. case MSPRO_BLOCK_ID_SPECFILEVALUES2:
  224. return "attr_specfilevalues2";
  225. case MSPRO_BLOCK_ID_DEVINFO:
  226. return "attr_devinfo";
  227. default:
  228. return NULL;
  229. };
  230. }
  231. typedef ssize_t (*sysfs_show_t)(struct device *dev,
  232. struct device_attribute *attr,
  233. char *buffer);
  234. static ssize_t mspro_block_attr_show_default(struct device *dev,
  235. struct device_attribute *attr,
  236. char *buffer)
  237. {
  238. struct mspro_sys_attr *s_attr = container_of(attr,
  239. struct mspro_sys_attr,
  240. dev_attr);
  241. ssize_t cnt, rc = 0;
  242. for (cnt = 0; cnt < s_attr->size; cnt++) {
  243. if (cnt && !(cnt % 16)) {
  244. if (PAGE_SIZE - rc)
  245. buffer[rc++] = '\n';
  246. }
  247. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "%02x ",
  248. ((unsigned char *)s_attr->data)[cnt]);
  249. }
  250. return rc;
  251. }
  252. static ssize_t mspro_block_attr_show_sysinfo(struct device *dev,
  253. struct device_attribute *attr,
  254. char *buffer)
  255. {
  256. struct mspro_sys_attr *x_attr = container_of(attr,
  257. struct mspro_sys_attr,
  258. dev_attr);
  259. struct mspro_sys_info *x_sys = x_attr->data;
  260. ssize_t rc = 0;
  261. int date_tz = 0, date_tz_f = 0;
  262. if (x_sys->assembly_date[0] > 0x80U) {
  263. date_tz = (~x_sys->assembly_date[0]) + 1;
  264. date_tz_f = date_tz & 3;
  265. date_tz >>= 2;
  266. date_tz = -date_tz;
  267. date_tz_f *= 15;
  268. } else if (x_sys->assembly_date[0] < 0x80U) {
  269. date_tz = x_sys->assembly_date[0];
  270. date_tz_f = date_tz & 3;
  271. date_tz >>= 2;
  272. date_tz_f *= 15;
  273. }
  274. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "class: %x\n",
  275. x_sys->class);
  276. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "block size: %x\n",
  277. be16_to_cpu(x_sys->block_size));
  278. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "block count: %x\n",
  279. be16_to_cpu(x_sys->block_count));
  280. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "user block count: %x\n",
  281. be16_to_cpu(x_sys->user_block_count));
  282. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "page size: %x\n",
  283. be16_to_cpu(x_sys->page_size));
  284. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "assembly date: "
  285. "GMT%+d:%d %04u-%02u-%02u %02u:%02u:%02u\n",
  286. date_tz, date_tz_f,
  287. be16_to_cpu(*(unsigned short *)
  288. &x_sys->assembly_date[1]),
  289. x_sys->assembly_date[3], x_sys->assembly_date[4],
  290. x_sys->assembly_date[5], x_sys->assembly_date[6],
  291. x_sys->assembly_date[7]);
  292. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "serial number: %x\n",
  293. be32_to_cpu(x_sys->serial_number));
  294. rc += scnprintf(buffer + rc, PAGE_SIZE - rc,
  295. "assembly maker code: %x\n",
  296. x_sys->assembly_maker_code);
  297. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "assembly model code: "
  298. "%02x%02x%02x\n", x_sys->assembly_model_code[0],
  299. x_sys->assembly_model_code[1],
  300. x_sys->assembly_model_code[2]);
  301. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "memory maker code: %x\n",
  302. be16_to_cpu(x_sys->memory_maker_code));
  303. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "memory model code: %x\n",
  304. be16_to_cpu(x_sys->memory_model_code));
  305. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "vcc: %x\n",
  306. x_sys->vcc);
  307. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "vpp: %x\n",
  308. x_sys->vpp);
  309. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "controller number: %x\n",
  310. be16_to_cpu(x_sys->controller_number));
  311. rc += scnprintf(buffer + rc, PAGE_SIZE - rc,
  312. "controller function: %x\n",
  313. be16_to_cpu(x_sys->controller_function));
  314. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start sector: %x\n",
  315. be16_to_cpu(x_sys->start_sector));
  316. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "unit size: %x\n",
  317. be16_to_cpu(x_sys->unit_size));
  318. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "sub class: %x\n",
  319. x_sys->ms_sub_class);
  320. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "interface type: %x\n",
  321. x_sys->interface_type);
  322. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "controller code: %x\n",
  323. be16_to_cpu(x_sys->controller_code));
  324. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "format type: %x\n",
  325. x_sys->format_type);
  326. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "device type: %x\n",
  327. x_sys->device_type);
  328. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "mspro id: %s\n",
  329. x_sys->mspro_id);
  330. return rc;
  331. }
  332. static ssize_t mspro_block_attr_show_modelname(struct device *dev,
  333. struct device_attribute *attr,
  334. char *buffer)
  335. {
  336. struct mspro_sys_attr *s_attr = container_of(attr,
  337. struct mspro_sys_attr,
  338. dev_attr);
  339. return scnprintf(buffer, PAGE_SIZE, "%s", (char *)s_attr->data);
  340. }
  341. static ssize_t mspro_block_attr_show_mbr(struct device *dev,
  342. struct device_attribute *attr,
  343. char *buffer)
  344. {
  345. struct mspro_sys_attr *x_attr = container_of(attr,
  346. struct mspro_sys_attr,
  347. dev_attr);
  348. struct mspro_mbr *x_mbr = x_attr->data;
  349. ssize_t rc = 0;
  350. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "boot partition: %x\n",
  351. x_mbr->boot_partition);
  352. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start head: %x\n",
  353. x_mbr->start_head);
  354. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start sector: %x\n",
  355. x_mbr->start_sector);
  356. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start cylinder: %x\n",
  357. x_mbr->start_cylinder);
  358. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "partition type: %x\n",
  359. x_mbr->partition_type);
  360. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "end head: %x\n",
  361. x_mbr->end_head);
  362. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "end sector: %x\n",
  363. x_mbr->end_sector);
  364. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "end cylinder: %x\n",
  365. x_mbr->end_cylinder);
  366. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start sectors: %x\n",
  367. x_mbr->start_sectors);
  368. rc += scnprintf(buffer + rc, PAGE_SIZE - rc,
  369. "sectors per partition: %x\n",
  370. x_mbr->sectors_per_partition);
  371. return rc;
  372. }
  373. static ssize_t mspro_block_attr_show_specfile(struct device *dev,
  374. struct device_attribute *attr,
  375. char *buffer)
  376. {
  377. struct mspro_sys_attr *x_attr = container_of(attr,
  378. struct mspro_sys_attr,
  379. dev_attr);
  380. struct mspro_specfile *x_spfile = x_attr->data;
  381. char name[9], ext[4];
  382. ssize_t rc = 0;
  383. memcpy(name, x_spfile->name, 8);
  384. name[8] = 0;
  385. memcpy(ext, x_spfile->ext, 3);
  386. ext[3] = 0;
  387. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "name: %s\n", name);
  388. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "ext: %s\n", ext);
  389. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "attribute: %x\n",
  390. x_spfile->attr);
  391. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "time: %d:%d:%d\n",
  392. x_spfile->time >> 11,
  393. (x_spfile->time >> 5) & 0x3f,
  394. (x_spfile->time & 0x1f) * 2);
  395. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "date: %d-%d-%d\n",
  396. (x_spfile->date >> 9) + 1980,
  397. (x_spfile->date >> 5) & 0xf,
  398. x_spfile->date & 0x1f);
  399. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "start cluster: %x\n",
  400. x_spfile->cluster);
  401. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "size: %x\n",
  402. x_spfile->size);
  403. return rc;
  404. }
  405. static ssize_t mspro_block_attr_show_devinfo(struct device *dev,
  406. struct device_attribute *attr,
  407. char *buffer)
  408. {
  409. struct mspro_sys_attr *x_attr = container_of(attr,
  410. struct mspro_sys_attr,
  411. dev_attr);
  412. struct mspro_devinfo *x_devinfo = x_attr->data;
  413. ssize_t rc = 0;
  414. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "cylinders: %x\n",
  415. be16_to_cpu(x_devinfo->cylinders));
  416. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "heads: %x\n",
  417. be16_to_cpu(x_devinfo->heads));
  418. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "bytes per track: %x\n",
  419. be16_to_cpu(x_devinfo->bytes_per_track));
  420. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "bytes per sector: %x\n",
  421. be16_to_cpu(x_devinfo->bytes_per_sector));
  422. rc += scnprintf(buffer + rc, PAGE_SIZE - rc, "sectors per track: %x\n",
  423. be16_to_cpu(x_devinfo->sectors_per_track));
  424. return rc;
  425. }
  426. static sysfs_show_t mspro_block_attr_show(unsigned char tag)
  427. {
  428. switch (tag) {
  429. case MSPRO_BLOCK_ID_SYSINFO:
  430. return mspro_block_attr_show_sysinfo;
  431. case MSPRO_BLOCK_ID_MODELNAME:
  432. return mspro_block_attr_show_modelname;
  433. case MSPRO_BLOCK_ID_MBR:
  434. return mspro_block_attr_show_mbr;
  435. case MSPRO_BLOCK_ID_SPECFILEVALUES1:
  436. case MSPRO_BLOCK_ID_SPECFILEVALUES2:
  437. return mspro_block_attr_show_specfile;
  438. case MSPRO_BLOCK_ID_DEVINFO:
  439. return mspro_block_attr_show_devinfo;
  440. default:
  441. return mspro_block_attr_show_default;
  442. }
  443. }
  444. /*** Protocol handlers ***/
  445. /*
  446. * Functions prefixed with "h_" are protocol callbacks. They can be called from
  447. * interrupt context. Return value of 0 means that request processing is still
  448. * ongoing, while special error value of -EAGAIN means that current request is
  449. * finished (and request processor should come back some time later).
  450. */
  451. static int h_mspro_block_req_init(struct memstick_dev *card,
  452. struct memstick_request **mrq)
  453. {
  454. struct mspro_block_data *msb = memstick_get_drvdata(card);
  455. *mrq = &card->current_mrq;
  456. card->next_request = msb->mrq_handler;
  457. return 0;
  458. }
  459. static int h_mspro_block_default(struct memstick_dev *card,
  460. struct memstick_request **mrq)
  461. {
  462. complete(&card->mrq_complete);
  463. if (!(*mrq)->error)
  464. return -EAGAIN;
  465. else
  466. return (*mrq)->error;
  467. }
  468. static int h_mspro_block_get_ro(struct memstick_dev *card,
  469. struct memstick_request **mrq)
  470. {
  471. struct mspro_block_data *msb = memstick_get_drvdata(card);
  472. if ((*mrq)->error) {
  473. complete(&card->mrq_complete);
  474. return (*mrq)->error;
  475. }
  476. if ((*mrq)->data[offsetof(struct ms_status_register, status0)]
  477. & MEMSTICK_STATUS0_WP)
  478. msb->read_only = 1;
  479. else
  480. msb->read_only = 0;
  481. complete(&card->mrq_complete);
  482. return -EAGAIN;
  483. }
  484. static int h_mspro_block_wait_for_ced(struct memstick_dev *card,
  485. struct memstick_request **mrq)
  486. {
  487. if ((*mrq)->error) {
  488. complete(&card->mrq_complete);
  489. return (*mrq)->error;
  490. }
  491. dev_dbg(&card->dev, "wait for ced: value %x\n", (*mrq)->data[0]);
  492. if ((*mrq)->data[0] & (MEMSTICK_INT_CMDNAK | MEMSTICK_INT_ERR)) {
  493. card->current_mrq.error = -EFAULT;
  494. complete(&card->mrq_complete);
  495. return card->current_mrq.error;
  496. }
  497. if (!((*mrq)->data[0] & MEMSTICK_INT_CED))
  498. return 0;
  499. else {
  500. card->current_mrq.error = 0;
  501. complete(&card->mrq_complete);
  502. return -EAGAIN;
  503. }
  504. }
  505. static int h_mspro_block_transfer_data(struct memstick_dev *card,
  506. struct memstick_request **mrq)
  507. {
  508. struct memstick_host *host = card->host;
  509. struct mspro_block_data *msb = memstick_get_drvdata(card);
  510. unsigned char t_val = 0;
  511. struct scatterlist t_sg = { 0 };
  512. size_t t_offset;
  513. if ((*mrq)->error) {
  514. complete(&card->mrq_complete);
  515. return (*mrq)->error;
  516. }
  517. switch ((*mrq)->tpc) {
  518. case MS_TPC_WRITE_REG:
  519. memstick_init_req(*mrq, MS_TPC_SET_CMD, &msb->transfer_cmd, 1);
  520. (*mrq)->get_int_reg = 1;
  521. return 0;
  522. case MS_TPC_SET_CMD:
  523. t_val = (*mrq)->int_reg;
  524. memstick_init_req(*mrq, MS_TPC_GET_INT, NULL, 1);
  525. if (host->caps & MEMSTICK_CAP_AUTO_GET_INT)
  526. goto has_int_reg;
  527. return 0;
  528. case MS_TPC_GET_INT:
  529. t_val = (*mrq)->data[0];
  530. has_int_reg:
  531. if (t_val & (MEMSTICK_INT_CMDNAK | MEMSTICK_INT_ERR)) {
  532. t_val = MSPRO_CMD_STOP;
  533. memstick_init_req(*mrq, MS_TPC_SET_CMD, &t_val, 1);
  534. card->next_request = h_mspro_block_default;
  535. return 0;
  536. }
  537. if (msb->current_page
  538. == (msb->req_sg[msb->current_seg].length
  539. / msb->page_size)) {
  540. msb->current_page = 0;
  541. msb->current_seg++;
  542. if (msb->current_seg == msb->seg_count) {
  543. if (t_val & MEMSTICK_INT_CED) {
  544. complete(&card->mrq_complete);
  545. return -EAGAIN;
  546. } else {
  547. card->next_request
  548. = h_mspro_block_wait_for_ced;
  549. memstick_init_req(*mrq, MS_TPC_GET_INT,
  550. NULL, 1);
  551. return 0;
  552. }
  553. }
  554. }
  555. if (!(t_val & MEMSTICK_INT_BREQ)) {
  556. memstick_init_req(*mrq, MS_TPC_GET_INT, NULL, 1);
  557. return 0;
  558. }
  559. t_offset = msb->req_sg[msb->current_seg].offset;
  560. t_offset += msb->current_page * msb->page_size;
  561. sg_set_page(&t_sg,
  562. nth_page(sg_page(&(msb->req_sg[msb->current_seg])),
  563. t_offset >> PAGE_SHIFT),
  564. msb->page_size, offset_in_page(t_offset));
  565. memstick_init_req_sg(*mrq, msb->data_dir == READ
  566. ? MS_TPC_READ_LONG_DATA
  567. : MS_TPC_WRITE_LONG_DATA,
  568. &t_sg);
  569. (*mrq)->get_int_reg = 1;
  570. return 0;
  571. case MS_TPC_READ_LONG_DATA:
  572. case MS_TPC_WRITE_LONG_DATA:
  573. msb->current_page++;
  574. if (host->caps & MEMSTICK_CAP_AUTO_GET_INT) {
  575. t_val = (*mrq)->int_reg;
  576. goto has_int_reg;
  577. } else {
  578. memstick_init_req(*mrq, MS_TPC_GET_INT, NULL, 1);
  579. return 0;
  580. }
  581. default:
  582. BUG();
  583. }
  584. }
  585. /*** Data transfer ***/
  586. static void mspro_block_process_request(struct memstick_dev *card,
  587. struct request *req)
  588. {
  589. struct mspro_block_data *msb = memstick_get_drvdata(card);
  590. struct mspro_param_register param;
  591. int rc, chunk, cnt;
  592. unsigned short page_count;
  593. sector_t t_sec;
  594. unsigned long flags;
  595. do {
  596. page_count = 0;
  597. msb->current_seg = 0;
  598. msb->seg_count = blk_rq_map_sg(req->q, req, msb->req_sg);
  599. if (msb->seg_count) {
  600. msb->current_page = 0;
  601. for (rc = 0; rc < msb->seg_count; rc++)
  602. page_count += msb->req_sg[rc].length
  603. / msb->page_size;
  604. t_sec = req->sector;
  605. sector_div(t_sec, msb->page_size >> 9);
  606. param.system = msb->system;
  607. param.data_count = cpu_to_be16(page_count);
  608. param.data_address = cpu_to_be32((uint32_t)t_sec);
  609. param.tpc_param = 0;
  610. msb->data_dir = rq_data_dir(req);
  611. msb->transfer_cmd = msb->data_dir == READ
  612. ? MSPRO_CMD_READ_DATA
  613. : MSPRO_CMD_WRITE_DATA;
  614. dev_dbg(&card->dev, "data transfer: cmd %x, "
  615. "lba %x, count %x\n", msb->transfer_cmd,
  616. be32_to_cpu(param.data_address),
  617. page_count);
  618. card->next_request = h_mspro_block_req_init;
  619. msb->mrq_handler = h_mspro_block_transfer_data;
  620. memstick_init_req(&card->current_mrq, MS_TPC_WRITE_REG,
  621. &param, sizeof(param));
  622. memstick_new_req(card->host);
  623. wait_for_completion(&card->mrq_complete);
  624. rc = card->current_mrq.error;
  625. if (rc || (card->current_mrq.tpc == MSPRO_CMD_STOP)) {
  626. for (cnt = 0; cnt < msb->current_seg; cnt++)
  627. page_count += msb->req_sg[cnt].length
  628. / msb->page_size;
  629. if (msb->current_page)
  630. page_count += msb->current_page - 1;
  631. if (page_count && (msb->data_dir == READ))
  632. rc = msb->page_size * page_count;
  633. else
  634. rc = -EIO;
  635. } else
  636. rc = msb->page_size * page_count;
  637. } else
  638. rc = -EFAULT;
  639. spin_lock_irqsave(&msb->q_lock, flags);
  640. if (rc >= 0)
  641. chunk = __blk_end_request(req, 0, rc);
  642. else
  643. chunk = __blk_end_request(req, rc, 0);
  644. dev_dbg(&card->dev, "end chunk %d, %d\n", rc, chunk);
  645. spin_unlock_irqrestore(&msb->q_lock, flags);
  646. } while (chunk);
  647. }
  648. static int mspro_block_has_request(struct mspro_block_data *msb)
  649. {
  650. int rc = 0;
  651. unsigned long flags;
  652. spin_lock_irqsave(&msb->q_lock, flags);
  653. if (kthread_should_stop() || msb->has_request)
  654. rc = 1;
  655. spin_unlock_irqrestore(&msb->q_lock, flags);
  656. return rc;
  657. }
  658. static int mspro_block_queue_thread(void *data)
  659. {
  660. struct memstick_dev *card = data;
  661. struct memstick_host *host = card->host;
  662. struct mspro_block_data *msb = memstick_get_drvdata(card);
  663. struct request *req;
  664. unsigned long flags;
  665. while (1) {
  666. wait_event(msb->q_wait, mspro_block_has_request(msb));
  667. dev_dbg(&card->dev, "thread iter\n");
  668. spin_lock_irqsave(&msb->q_lock, flags);
  669. req = elv_next_request(msb->queue);
  670. dev_dbg(&card->dev, "next req %p\n", req);
  671. if (!req) {
  672. msb->has_request = 0;
  673. if (kthread_should_stop()) {
  674. spin_unlock_irqrestore(&msb->q_lock, flags);
  675. break;
  676. }
  677. } else
  678. msb->has_request = 1;
  679. spin_unlock_irqrestore(&msb->q_lock, flags);
  680. if (req) {
  681. mutex_lock(&host->lock);
  682. mspro_block_process_request(card, req);
  683. mutex_unlock(&host->lock);
  684. }
  685. }
  686. dev_dbg(&card->dev, "thread finished\n");
  687. return 0;
  688. }
  689. static void mspro_block_request(struct request_queue *q)
  690. {
  691. struct memstick_dev *card = q->queuedata;
  692. struct mspro_block_data *msb = memstick_get_drvdata(card);
  693. struct request *req = NULL;
  694. if (msb->q_thread) {
  695. msb->has_request = 1;
  696. wake_up_all(&msb->q_wait);
  697. } else {
  698. while ((req = elv_next_request(q)) != NULL)
  699. end_queued_request(req, -ENODEV);
  700. }
  701. }
  702. /*** Initialization ***/
  703. static int mspro_block_wait_for_ced(struct memstick_dev *card)
  704. {
  705. struct mspro_block_data *msb = memstick_get_drvdata(card);
  706. card->next_request = h_mspro_block_req_init;
  707. msb->mrq_handler = h_mspro_block_wait_for_ced;
  708. memstick_init_req(&card->current_mrq, MS_TPC_GET_INT, NULL, 1);
  709. memstick_new_req(card->host);
  710. wait_for_completion(&card->mrq_complete);
  711. return card->current_mrq.error;
  712. }
  713. static int mspro_block_switch_to_parallel(struct memstick_dev *card)
  714. {
  715. struct memstick_host *host = card->host;
  716. struct mspro_block_data *msb = memstick_get_drvdata(card);
  717. struct mspro_param_register param = {
  718. .system = MEMSTICK_SYS_PAR4,
  719. .data_count = 0,
  720. .data_address = 0,
  721. .tpc_param = 0
  722. };
  723. card->next_request = h_mspro_block_req_init;
  724. msb->mrq_handler = h_mspro_block_default;
  725. memstick_init_req(&card->current_mrq, MS_TPC_WRITE_REG, &param,
  726. sizeof(param));
  727. memstick_new_req(host);
  728. wait_for_completion(&card->mrq_complete);
  729. if (card->current_mrq.error)
  730. return card->current_mrq.error;
  731. msb->system = MEMSTICK_SYS_PAR4;
  732. host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_PAR4);
  733. card->next_request = h_mspro_block_req_init;
  734. msb->mrq_handler = h_mspro_block_default;
  735. memstick_init_req(&card->current_mrq, MS_TPC_GET_INT, NULL, 1);
  736. memstick_new_req(card->host);
  737. wait_for_completion(&card->mrq_complete);
  738. if (card->current_mrq.error) {
  739. msb->system = MEMSTICK_SYS_SERIAL;
  740. host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_OFF);
  741. msleep(1000);
  742. host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_ON);
  743. host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_SERIAL);
  744. if (memstick_set_rw_addr(card))
  745. return card->current_mrq.error;
  746. param.system = msb->system;
  747. card->next_request = h_mspro_block_req_init;
  748. msb->mrq_handler = h_mspro_block_default;
  749. memstick_init_req(&card->current_mrq, MS_TPC_WRITE_REG, &param,
  750. sizeof(param));
  751. memstick_new_req(host);
  752. wait_for_completion(&card->mrq_complete);
  753. return -EFAULT;
  754. }
  755. return 0;
  756. }
  757. /* Memory allocated for attributes by this function should be freed by
  758. * mspro_block_data_clear, no matter if the initialization process succeded
  759. * or failed.
  760. */
  761. static int mspro_block_read_attributes(struct memstick_dev *card)
  762. {
  763. struct mspro_block_data *msb = memstick_get_drvdata(card);
  764. struct mspro_param_register param = {
  765. .system = msb->system,
  766. .data_count = cpu_to_be16(1),
  767. .data_address = 0,
  768. .tpc_param = 0
  769. };
  770. struct mspro_attribute *attr = NULL;
  771. struct mspro_sys_attr *s_attr = NULL;
  772. unsigned char *buffer = NULL;
  773. int cnt, rc, attr_count;
  774. unsigned int addr;
  775. unsigned short page_count;
  776. attr = kmalloc(msb->page_size, GFP_KERNEL);
  777. if (!attr)
  778. return -ENOMEM;
  779. sg_init_one(&msb->req_sg[0], attr, msb->page_size);
  780. msb->seg_count = 1;
  781. msb->current_seg = 0;
  782. msb->current_page = 0;
  783. msb->data_dir = READ;
  784. msb->transfer_cmd = MSPRO_CMD_READ_ATRB;
  785. card->next_request = h_mspro_block_req_init;
  786. msb->mrq_handler = h_mspro_block_transfer_data;
  787. memstick_init_req(&card->current_mrq, MS_TPC_WRITE_REG, &param,
  788. sizeof(param));
  789. memstick_new_req(card->host);
  790. wait_for_completion(&card->mrq_complete);
  791. if (card->current_mrq.error) {
  792. rc = card->current_mrq.error;
  793. goto out_free_attr;
  794. }
  795. if (be16_to_cpu(attr->signature) != MSPRO_BLOCK_SIGNATURE) {
  796. printk(KERN_ERR "%s: unrecognized device signature %x\n",
  797. card->dev.bus_id, be16_to_cpu(attr->signature));
  798. rc = -ENODEV;
  799. goto out_free_attr;
  800. }
  801. if (attr->count > MSPRO_BLOCK_MAX_ATTRIBUTES) {
  802. printk(KERN_WARNING "%s: way too many attribute entries\n",
  803. card->dev.bus_id);
  804. attr_count = MSPRO_BLOCK_MAX_ATTRIBUTES;
  805. } else
  806. attr_count = attr->count;
  807. msb->attr_group.attrs = kzalloc((attr_count + 1)
  808. * sizeof(struct attribute),
  809. GFP_KERNEL);
  810. if (!msb->attr_group.attrs) {
  811. rc = -ENOMEM;
  812. goto out_free_attr;
  813. }
  814. msb->attr_group.name = "media_attributes";
  815. buffer = kmalloc(msb->page_size, GFP_KERNEL);
  816. if (!buffer) {
  817. rc = -ENOMEM;
  818. goto out_free_attr;
  819. }
  820. memcpy(buffer, (char *)attr, msb->page_size);
  821. page_count = 1;
  822. for (cnt = 0; cnt < attr_count; ++cnt) {
  823. s_attr = kzalloc(sizeof(struct mspro_sys_attr), GFP_KERNEL);
  824. if (!s_attr) {
  825. rc = -ENOMEM;
  826. goto out_free_buffer;
  827. }
  828. msb->attr_group.attrs[cnt] = &s_attr->dev_attr.attr;
  829. addr = be32_to_cpu(attr->entries[cnt].address);
  830. rc = be32_to_cpu(attr->entries[cnt].size);
  831. dev_dbg(&card->dev, "adding attribute %d: id %x, address %x, "
  832. "size %x\n", cnt, attr->entries[cnt].id, addr, rc);
  833. s_attr->id = attr->entries[cnt].id;
  834. if (mspro_block_attr_name(s_attr->id))
  835. snprintf(s_attr->name, sizeof(s_attr->name), "%s",
  836. mspro_block_attr_name(attr->entries[cnt].id));
  837. else
  838. snprintf(s_attr->name, sizeof(s_attr->name),
  839. "attr_x%02x", attr->entries[cnt].id);
  840. s_attr->dev_attr.attr.name = s_attr->name;
  841. s_attr->dev_attr.attr.mode = S_IRUGO;
  842. s_attr->dev_attr.attr.owner = THIS_MODULE;
  843. s_attr->dev_attr.show = mspro_block_attr_show(s_attr->id);
  844. if (!rc)
  845. continue;
  846. s_attr->size = rc;
  847. s_attr->data = kmalloc(rc, GFP_KERNEL);
  848. if (!s_attr->data) {
  849. rc = -ENOMEM;
  850. goto out_free_buffer;
  851. }
  852. if (((addr / msb->page_size)
  853. == be32_to_cpu(param.data_address))
  854. && (((addr + rc - 1) / msb->page_size)
  855. == be32_to_cpu(param.data_address))) {
  856. memcpy(s_attr->data, buffer + addr % msb->page_size,
  857. rc);
  858. continue;
  859. }
  860. if (page_count <= (rc / msb->page_size)) {
  861. kfree(buffer);
  862. page_count = (rc / msb->page_size) + 1;
  863. buffer = kmalloc(page_count * msb->page_size,
  864. GFP_KERNEL);
  865. if (!buffer) {
  866. rc = -ENOMEM;
  867. goto out_free_attr;
  868. }
  869. }
  870. param.system = msb->system;
  871. param.data_count = cpu_to_be16((rc / msb->page_size) + 1);
  872. param.data_address = cpu_to_be32(addr / msb->page_size);
  873. param.tpc_param = 0;
  874. sg_init_one(&msb->req_sg[0], buffer,
  875. be16_to_cpu(param.data_count) * msb->page_size);
  876. msb->seg_count = 1;
  877. msb->current_seg = 0;
  878. msb->current_page = 0;
  879. msb->data_dir = READ;
  880. msb->transfer_cmd = MSPRO_CMD_READ_ATRB;
  881. dev_dbg(&card->dev, "reading attribute pages %x, %x\n",
  882. be32_to_cpu(param.data_address),
  883. be16_to_cpu(param.data_count));
  884. card->next_request = h_mspro_block_req_init;
  885. msb->mrq_handler = h_mspro_block_transfer_data;
  886. memstick_init_req(&card->current_mrq, MS_TPC_WRITE_REG,
  887. (char *)&param, sizeof(param));
  888. memstick_new_req(card->host);
  889. wait_for_completion(&card->mrq_complete);
  890. if (card->current_mrq.error) {
  891. rc = card->current_mrq.error;
  892. goto out_free_buffer;
  893. }
  894. memcpy(s_attr->data, buffer + addr % msb->page_size, rc);
  895. }
  896. rc = 0;
  897. out_free_buffer:
  898. kfree(buffer);
  899. out_free_attr:
  900. kfree(attr);
  901. return rc;
  902. }
  903. static int mspro_block_init_card(struct memstick_dev *card)
  904. {
  905. struct mspro_block_data *msb = memstick_get_drvdata(card);
  906. struct memstick_host *host = card->host;
  907. int rc = 0;
  908. msb->system = MEMSTICK_SYS_SERIAL;
  909. card->reg_addr.r_offset = offsetof(struct mspro_register, status);
  910. card->reg_addr.r_length = sizeof(struct ms_status_register);
  911. card->reg_addr.w_offset = offsetof(struct mspro_register, param);
  912. card->reg_addr.w_length = sizeof(struct mspro_param_register);
  913. if (memstick_set_rw_addr(card))
  914. return -EIO;
  915. if (host->caps & MEMSTICK_CAP_PAR4) {
  916. if (mspro_block_switch_to_parallel(card))
  917. printk(KERN_WARNING "%s: could not switch to "
  918. "parallel interface\n", card->dev.bus_id);
  919. }
  920. rc = mspro_block_wait_for_ced(card);
  921. if (rc)
  922. return rc;
  923. dev_dbg(&card->dev, "card activated\n");
  924. card->next_request = h_mspro_block_req_init;
  925. msb->mrq_handler = h_mspro_block_get_ro;
  926. memstick_init_req(&card->current_mrq, MS_TPC_READ_REG, NULL,
  927. sizeof(struct ms_status_register));
  928. memstick_new_req(card->host);
  929. wait_for_completion(&card->mrq_complete);
  930. if (card->current_mrq.error)
  931. return card->current_mrq.error;
  932. dev_dbg(&card->dev, "card r/w status %d\n", msb->read_only ? 0 : 1);
  933. msb->page_size = 512;
  934. rc = mspro_block_read_attributes(card);
  935. if (rc)
  936. return rc;
  937. dev_dbg(&card->dev, "attributes loaded\n");
  938. return 0;
  939. }
  940. static int mspro_block_init_disk(struct memstick_dev *card)
  941. {
  942. struct mspro_block_data *msb = memstick_get_drvdata(card);
  943. struct memstick_host *host = card->host;
  944. struct mspro_devinfo *dev_info = NULL;
  945. struct mspro_sys_info *sys_info = NULL;
  946. struct mspro_sys_attr *s_attr = NULL;
  947. int rc, disk_id;
  948. u64 limit = BLK_BOUNCE_HIGH;
  949. unsigned long capacity;
  950. if (host->cdev.dev->dma_mask && *(host->cdev.dev->dma_mask))
  951. limit = *(host->cdev.dev->dma_mask);
  952. for (rc = 0; msb->attr_group.attrs[rc]; ++rc) {
  953. s_attr = mspro_from_sysfs_attr(msb->attr_group.attrs[rc]);
  954. if (s_attr->id == MSPRO_BLOCK_ID_DEVINFO)
  955. dev_info = s_attr->data;
  956. else if (s_attr->id == MSPRO_BLOCK_ID_SYSINFO)
  957. sys_info = s_attr->data;
  958. }
  959. if (!dev_info || !sys_info)
  960. return -ENODEV;
  961. msb->cylinders = be16_to_cpu(dev_info->cylinders);
  962. msb->heads = be16_to_cpu(dev_info->heads);
  963. msb->sectors_per_track = be16_to_cpu(dev_info->sectors_per_track);
  964. msb->page_size = be16_to_cpu(sys_info->unit_size);
  965. if (!idr_pre_get(&mspro_block_disk_idr, GFP_KERNEL))
  966. return -ENOMEM;
  967. mutex_lock(&mspro_block_disk_lock);
  968. rc = idr_get_new(&mspro_block_disk_idr, card, &disk_id);
  969. mutex_unlock(&mspro_block_disk_lock);
  970. if (rc)
  971. return rc;
  972. if ((disk_id << MEMSTICK_PART_SHIFT) > 255) {
  973. rc = -ENOSPC;
  974. goto out_release_id;
  975. }
  976. msb->disk = alloc_disk(1 << MEMSTICK_PART_SHIFT);
  977. if (!msb->disk) {
  978. rc = -ENOMEM;
  979. goto out_release_id;
  980. }
  981. spin_lock_init(&msb->q_lock);
  982. init_waitqueue_head(&msb->q_wait);
  983. msb->queue = blk_init_queue(mspro_block_request, &msb->q_lock);
  984. if (!msb->queue) {
  985. rc = -ENOMEM;
  986. goto out_put_disk;
  987. }
  988. msb->queue->queuedata = card;
  989. blk_queue_bounce_limit(msb->queue, limit);
  990. blk_queue_max_sectors(msb->queue, MSPRO_BLOCK_MAX_PAGES);
  991. blk_queue_max_phys_segments(msb->queue, MSPRO_BLOCK_MAX_SEGS);
  992. blk_queue_max_hw_segments(msb->queue, MSPRO_BLOCK_MAX_SEGS);
  993. blk_queue_max_segment_size(msb->queue,
  994. MSPRO_BLOCK_MAX_PAGES * msb->page_size);
  995. msb->disk->major = major;
  996. msb->disk->first_minor = disk_id << MEMSTICK_PART_SHIFT;
  997. msb->disk->fops = &ms_block_bdops;
  998. msb->usage_count = 1;
  999. msb->disk->private_data = msb;
  1000. msb->disk->queue = msb->queue;
  1001. msb->disk->driverfs_dev = &card->dev;
  1002. sprintf(msb->disk->disk_name, "mspblk%d", disk_id);
  1003. blk_queue_hardsect_size(msb->queue, msb->page_size);
  1004. capacity = be16_to_cpu(sys_info->user_block_count);
  1005. capacity *= be16_to_cpu(sys_info->block_size);
  1006. capacity *= msb->page_size >> 9;
  1007. set_capacity(msb->disk, capacity);
  1008. dev_dbg(&card->dev, "capacity set %ld\n", capacity);
  1009. msb->q_thread = kthread_run(mspro_block_queue_thread, card,
  1010. DRIVER_NAME"d");
  1011. if (IS_ERR(msb->q_thread))
  1012. goto out_put_disk;
  1013. mutex_unlock(&host->lock);
  1014. add_disk(msb->disk);
  1015. mutex_lock(&host->lock);
  1016. msb->active = 1;
  1017. return 0;
  1018. out_put_disk:
  1019. put_disk(msb->disk);
  1020. out_release_id:
  1021. mutex_lock(&mspro_block_disk_lock);
  1022. idr_remove(&mspro_block_disk_idr, disk_id);
  1023. mutex_unlock(&mspro_block_disk_lock);
  1024. return rc;
  1025. }
  1026. static void mspro_block_data_clear(struct mspro_block_data *msb)
  1027. {
  1028. int cnt;
  1029. struct mspro_sys_attr *s_attr;
  1030. if (msb->attr_group.attrs) {
  1031. for (cnt = 0; msb->attr_group.attrs[cnt]; ++cnt) {
  1032. s_attr = mspro_from_sysfs_attr(msb->attr_group
  1033. .attrs[cnt]);
  1034. kfree(s_attr->data);
  1035. kfree(s_attr);
  1036. }
  1037. kfree(msb->attr_group.attrs);
  1038. }
  1039. msb->card = NULL;
  1040. }
  1041. static int mspro_block_check_card(struct memstick_dev *card)
  1042. {
  1043. struct mspro_block_data *msb = memstick_get_drvdata(card);
  1044. return (msb->active == 1);
  1045. }
  1046. static int mspro_block_probe(struct memstick_dev *card)
  1047. {
  1048. struct mspro_block_data *msb;
  1049. int rc = 0;
  1050. msb = kzalloc(sizeof(struct mspro_block_data), GFP_KERNEL);
  1051. if (!msb)
  1052. return -ENOMEM;
  1053. memstick_set_drvdata(card, msb);
  1054. msb->card = card;
  1055. rc = mspro_block_init_card(card);
  1056. if (rc)
  1057. goto out_free;
  1058. rc = sysfs_create_group(&card->dev.kobj, &msb->attr_group);
  1059. if (rc)
  1060. goto out_free;
  1061. rc = mspro_block_init_disk(card);
  1062. if (!rc) {
  1063. card->check = mspro_block_check_card;
  1064. return 0;
  1065. }
  1066. sysfs_remove_group(&card->dev.kobj, &msb->attr_group);
  1067. out_free:
  1068. memstick_set_drvdata(card, NULL);
  1069. mspro_block_data_clear(msb);
  1070. kfree(msb);
  1071. return rc;
  1072. }
  1073. static void mspro_block_remove(struct memstick_dev *card)
  1074. {
  1075. struct mspro_block_data *msb = memstick_get_drvdata(card);
  1076. struct task_struct *q_thread = NULL;
  1077. unsigned long flags;
  1078. del_gendisk(msb->disk);
  1079. dev_dbg(&card->dev, "mspro block remove\n");
  1080. spin_lock_irqsave(&msb->q_lock, flags);
  1081. q_thread = msb->q_thread;
  1082. msb->q_thread = NULL;
  1083. msb->active = 0;
  1084. spin_unlock_irqrestore(&msb->q_lock, flags);
  1085. if (q_thread) {
  1086. mutex_unlock(&card->host->lock);
  1087. kthread_stop(q_thread);
  1088. mutex_lock(&card->host->lock);
  1089. }
  1090. dev_dbg(&card->dev, "queue thread stopped\n");
  1091. blk_cleanup_queue(msb->queue);
  1092. sysfs_remove_group(&card->dev.kobj, &msb->attr_group);
  1093. mutex_lock(&mspro_block_disk_lock);
  1094. mspro_block_data_clear(msb);
  1095. mutex_unlock(&mspro_block_disk_lock);
  1096. mspro_block_disk_release(msb->disk);
  1097. memstick_set_drvdata(card, NULL);
  1098. }
  1099. #ifdef CONFIG_PM
  1100. static int mspro_block_suspend(struct memstick_dev *card, pm_message_t state)
  1101. {
  1102. struct mspro_block_data *msb = memstick_get_drvdata(card);
  1103. struct task_struct *q_thread = NULL;
  1104. unsigned long flags;
  1105. spin_lock_irqsave(&msb->q_lock, flags);
  1106. q_thread = msb->q_thread;
  1107. msb->q_thread = NULL;
  1108. msb->active = 0;
  1109. blk_stop_queue(msb->queue);
  1110. spin_unlock_irqrestore(&msb->q_lock, flags);
  1111. if (q_thread)
  1112. kthread_stop(q_thread);
  1113. return 0;
  1114. }
  1115. static int mspro_block_resume(struct memstick_dev *card)
  1116. {
  1117. struct mspro_block_data *msb = memstick_get_drvdata(card);
  1118. unsigned long flags;
  1119. int rc = 0;
  1120. #ifdef CONFIG_MEMSTICK_UNSAFE_RESUME
  1121. struct mspro_block_data *new_msb;
  1122. struct memstick_host *host = card->host;
  1123. struct mspro_sys_attr *s_attr, *r_attr;
  1124. unsigned char cnt;
  1125. mutex_lock(&host->lock);
  1126. new_msb = kzalloc(sizeof(struct mspro_block_data), GFP_KERNEL);
  1127. if (!new_msb) {
  1128. rc = -ENOMEM;
  1129. goto out_unlock;
  1130. }
  1131. new_msb->card = card;
  1132. memstick_set_drvdata(card, new_msb);
  1133. if (mspro_block_init_card(card))
  1134. goto out_free;
  1135. for (cnt = 0; new_msb->attr_group.attrs[cnt]
  1136. && msb->attr_group.attrs[cnt]; ++cnt) {
  1137. s_attr = mspro_from_sysfs_attr(new_msb->attr_group.attrs[cnt]);
  1138. r_attr = mspro_from_sysfs_attr(msb->attr_group.attrs[cnt]);
  1139. if (s_attr->id == MSPRO_BLOCK_ID_SYSINFO
  1140. && r_attr->id == s_attr->id) {
  1141. if (memcmp(s_attr->data, r_attr->data, s_attr->size))
  1142. break;
  1143. memstick_set_drvdata(card, msb);
  1144. msb->q_thread = kthread_run(mspro_block_queue_thread,
  1145. card, DRIVER_NAME"d");
  1146. if (IS_ERR(msb->q_thread))
  1147. msb->q_thread = NULL;
  1148. else
  1149. msb->active = 1;
  1150. break;
  1151. }
  1152. }
  1153. out_free:
  1154. memstick_set_drvdata(card, msb);
  1155. mspro_block_data_clear(new_msb);
  1156. kfree(new_msb);
  1157. out_unlock:
  1158. mutex_unlock(&host->lock);
  1159. #endif /* CONFIG_MEMSTICK_UNSAFE_RESUME */
  1160. spin_lock_irqsave(&msb->q_lock, flags);
  1161. blk_start_queue(msb->queue);
  1162. spin_unlock_irqrestore(&msb->q_lock, flags);
  1163. return rc;
  1164. }
  1165. #else
  1166. #define mspro_block_suspend NULL
  1167. #define mspro_block_resume NULL
  1168. #endif /* CONFIG_PM */
  1169. static struct memstick_device_id mspro_block_id_tbl[] = {
  1170. {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_PRO, MEMSTICK_CATEGORY_STORAGE_DUO,
  1171. MEMSTICK_CLASS_GENERIC_DUO},
  1172. {}
  1173. };
  1174. static struct memstick_driver mspro_block_driver = {
  1175. .driver = {
  1176. .name = DRIVER_NAME,
  1177. .owner = THIS_MODULE
  1178. },
  1179. .id_table = mspro_block_id_tbl,
  1180. .probe = mspro_block_probe,
  1181. .remove = mspro_block_remove,
  1182. .suspend = mspro_block_suspend,
  1183. .resume = mspro_block_resume
  1184. };
  1185. static int __init mspro_block_init(void)
  1186. {
  1187. int rc = -ENOMEM;
  1188. rc = register_blkdev(major, DRIVER_NAME);
  1189. if (rc < 0) {
  1190. printk(KERN_ERR DRIVER_NAME ": failed to register "
  1191. "major %d, error %d\n", major, rc);
  1192. return rc;
  1193. }
  1194. if (!major)
  1195. major = rc;
  1196. rc = memstick_register_driver(&mspro_block_driver);
  1197. if (rc)
  1198. unregister_blkdev(major, DRIVER_NAME);
  1199. return rc;
  1200. }
  1201. static void __exit mspro_block_exit(void)
  1202. {
  1203. memstick_unregister_driver(&mspro_block_driver);
  1204. unregister_blkdev(major, DRIVER_NAME);
  1205. idr_destroy(&mspro_block_disk_idr);
  1206. }
  1207. module_init(mspro_block_init);
  1208. module_exit(mspro_block_exit);
  1209. MODULE_LICENSE("GPL");
  1210. MODULE_AUTHOR("Alex Dubov");
  1211. MODULE_DESCRIPTION("Sony MemoryStickPro block device driver");
  1212. MODULE_DEVICE_TABLE(memstick, mspro_block_id_tbl);