1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /* |
3 | * Sony MemoryStick Pro storage support |
4 | * |
5 | * Copyright (C) 2007 Alex Dubov <oakad@yahoo.com> |
6 | * |
7 | * Special thanks to Carlos Corbacho for providing various MemoryStick cards |
8 | * that made this driver possible. |
9 | */ |
10 | |
11 | #include <linux/blk-mq.h> |
12 | #include <linux/idr.h> |
13 | #include <linux/hdreg.h> |
14 | #include <linux/kthread.h> |
15 | #include <linux/delay.h> |
16 | #include <linux/slab.h> |
17 | #include <linux/mutex.h> |
18 | #include <linux/memstick.h> |
19 | #include <linux/module.h> |
20 | |
21 | #define DRIVER_NAME "mspro_block" |
22 | |
23 | static int major; |
24 | module_param(major, int, 0644); |
25 | |
26 | #define MSPRO_BLOCK_MAX_SEGS 32 |
27 | #define MSPRO_BLOCK_MAX_PAGES ((2 << 16) - 1) |
28 | |
29 | #define MSPRO_BLOCK_SIGNATURE 0xa5c3 |
30 | #define MSPRO_BLOCK_MAX_ATTRIBUTES 41 |
31 | |
32 | #define MSPRO_BLOCK_PART_SHIFT 3 |
33 | |
34 | enum { |
35 | MSPRO_BLOCK_ID_SYSINFO = 0x10, |
36 | MSPRO_BLOCK_ID_MODELNAME = 0x15, |
37 | MSPRO_BLOCK_ID_MBR = 0x20, |
38 | MSPRO_BLOCK_ID_PBR16 = 0x21, |
39 | MSPRO_BLOCK_ID_PBR32 = 0x22, |
40 | MSPRO_BLOCK_ID_SPECFILEVALUES1 = 0x25, |
41 | MSPRO_BLOCK_ID_SPECFILEVALUES2 = 0x26, |
42 | MSPRO_BLOCK_ID_DEVINFO = 0x30 |
43 | }; |
44 | |
45 | struct mspro_sys_attr { |
46 | size_t size; |
47 | void *data; |
48 | unsigned char id; |
49 | char name[32]; |
50 | struct device_attribute dev_attr; |
51 | }; |
52 | |
53 | struct mspro_attr_entry { |
54 | __be32 address; |
55 | __be32 size; |
56 | unsigned char id; |
57 | unsigned char reserved[3]; |
58 | } __attribute__((packed)); |
59 | |
60 | struct mspro_attribute { |
61 | __be16 signature; |
62 | unsigned short version; |
63 | unsigned char count; |
64 | unsigned char reserved[11]; |
65 | struct mspro_attr_entry entries[]; |
66 | } __attribute__((packed)); |
67 | |
68 | struct mspro_sys_info { |
69 | unsigned char class; |
70 | unsigned char reserved0; |
71 | __be16 block_size; |
72 | __be16 block_count; |
73 | __be16 user_block_count; |
74 | __be16 page_size; |
75 | unsigned char reserved1[2]; |
76 | unsigned char assembly_date[8]; |
77 | __be32 serial_number; |
78 | unsigned char assembly_maker_code; |
79 | unsigned char assembly_model_code[3]; |
80 | __be16 memory_maker_code; |
81 | __be16 memory_model_code; |
82 | unsigned char reserved2[4]; |
83 | unsigned char vcc; |
84 | unsigned char vpp; |
85 | __be16 controller_number; |
86 | __be16 controller_function; |
87 | __be16 start_sector; |
88 | __be16 unit_size; |
89 | unsigned char ms_sub_class; |
90 | unsigned char reserved3[4]; |
91 | unsigned char interface_type; |
92 | __be16 controller_code; |
93 | unsigned char format_type; |
94 | unsigned char reserved4; |
95 | unsigned char device_type; |
96 | unsigned char reserved5[7]; |
97 | unsigned char mspro_id[16]; |
98 | unsigned char reserved6[16]; |
99 | } __attribute__((packed)); |
100 | |
101 | struct mspro_mbr { |
102 | unsigned char boot_partition; |
103 | unsigned char start_head; |
104 | unsigned char start_sector; |
105 | unsigned char start_cylinder; |
106 | unsigned char partition_type; |
107 | unsigned char end_head; |
108 | unsigned char end_sector; |
109 | unsigned char end_cylinder; |
110 | unsigned int start_sectors; |
111 | unsigned int sectors_per_partition; |
112 | } __attribute__((packed)); |
113 | |
114 | struct mspro_specfile { |
115 | char name[8]; |
116 | char ext[3]; |
117 | unsigned char attr; |
118 | unsigned char reserved[10]; |
119 | unsigned short time; |
120 | unsigned short date; |
121 | unsigned short cluster; |
122 | unsigned int size; |
123 | } __attribute__((packed)); |
124 | |
125 | struct mspro_devinfo { |
126 | __be16 cylinders; |
127 | __be16 heads; |
128 | __be16 bytes_per_track; |
129 | __be16 bytes_per_sector; |
130 | __be16 sectors_per_track; |
131 | unsigned char reserved[6]; |
132 | } __attribute__((packed)); |
133 | |
134 | struct mspro_block_data { |
135 | struct memstick_dev *card; |
136 | unsigned int caps; |
137 | struct gendisk *disk; |
138 | struct request_queue *queue; |
139 | struct request *block_req; |
140 | struct blk_mq_tag_set tag_set; |
141 | spinlock_t q_lock; |
142 | |
143 | unsigned short page_size; |
144 | unsigned short cylinders; |
145 | unsigned short heads; |
146 | unsigned short sectors_per_track; |
147 | |
148 | unsigned char system; |
149 | unsigned char read_only:1, |
150 | eject:1, |
151 | data_dir:1, |
152 | active:1; |
153 | unsigned char transfer_cmd; |
154 | |
155 | int (*mrq_handler)(struct memstick_dev *card, |
156 | struct memstick_request **mrq); |
157 | |
158 | |
159 | /* Default request setup function for data access method preferred by |
160 | * this host instance. |
161 | */ |
162 | void (*setup_transfer)(struct memstick_dev *card, |
163 | u64 offset, size_t length); |
164 | |
165 | struct attribute_group attr_group; |
166 | |
167 | struct scatterlist req_sg[MSPRO_BLOCK_MAX_SEGS]; |
168 | unsigned int seg_count; |
169 | unsigned int current_seg; |
170 | unsigned int current_page; |
171 | }; |
172 | |
173 | static DEFINE_IDR(mspro_block_disk_idr); |
174 | static DEFINE_MUTEX(mspro_block_disk_lock); |
175 | |
176 | static int mspro_block_complete_req(struct memstick_dev *card, int error); |
177 | |
178 | /*** Block device ***/ |
179 | |
180 | static void mspro_block_bd_free_disk(struct gendisk *disk) |
181 | { |
182 | struct mspro_block_data *msb = disk->private_data; |
183 | int disk_id = MINOR(disk_devt(disk)) >> MSPRO_BLOCK_PART_SHIFT; |
184 | |
185 | mutex_lock(&mspro_block_disk_lock); |
186 | idr_remove(&mspro_block_disk_idr, id: disk_id); |
187 | mutex_unlock(lock: &mspro_block_disk_lock); |
188 | |
189 | kfree(objp: msb); |
190 | } |
191 | |
192 | static int mspro_block_bd_getgeo(struct block_device *bdev, |
193 | struct hd_geometry *geo) |
194 | { |
195 | struct mspro_block_data *msb = bdev->bd_disk->private_data; |
196 | |
197 | geo->heads = msb->heads; |
198 | geo->sectors = msb->sectors_per_track; |
199 | geo->cylinders = msb->cylinders; |
200 | |
201 | return 0; |
202 | } |
203 | |
204 | static const struct block_device_operations ms_block_bdops = { |
205 | .owner = THIS_MODULE, |
206 | .getgeo = mspro_block_bd_getgeo, |
207 | .free_disk = mspro_block_bd_free_disk, |
208 | }; |
209 | |
210 | /*** Information ***/ |
211 | |
212 | static struct mspro_sys_attr *mspro_from_sysfs_attr(struct attribute *attr) |
213 | { |
214 | struct device_attribute *dev_attr |
215 | = container_of(attr, struct device_attribute, attr); |
216 | return container_of(dev_attr, struct mspro_sys_attr, dev_attr); |
217 | } |
218 | |
219 | static const char *mspro_block_attr_name(unsigned char tag) |
220 | { |
221 | switch (tag) { |
222 | case MSPRO_BLOCK_ID_SYSINFO: |
223 | return "attr_sysinfo" ; |
224 | case MSPRO_BLOCK_ID_MODELNAME: |
225 | return "attr_modelname" ; |
226 | case MSPRO_BLOCK_ID_MBR: |
227 | return "attr_mbr" ; |
228 | case MSPRO_BLOCK_ID_PBR16: |
229 | return "attr_pbr16" ; |
230 | case MSPRO_BLOCK_ID_PBR32: |
231 | return "attr_pbr32" ; |
232 | case MSPRO_BLOCK_ID_SPECFILEVALUES1: |
233 | return "attr_specfilevalues1" ; |
234 | case MSPRO_BLOCK_ID_SPECFILEVALUES2: |
235 | return "attr_specfilevalues2" ; |
236 | case MSPRO_BLOCK_ID_DEVINFO: |
237 | return "attr_devinfo" ; |
238 | default: |
239 | return NULL; |
240 | } |
241 | } |
242 | |
243 | typedef ssize_t (*sysfs_show_t)(struct device *dev, |
244 | struct device_attribute *attr, |
245 | char *buffer); |
246 | |
247 | static ssize_t mspro_block_attr_show_default(struct device *dev, |
248 | struct device_attribute *attr, |
249 | char *buffer) |
250 | { |
251 | struct mspro_sys_attr *s_attr = container_of(attr, |
252 | struct mspro_sys_attr, |
253 | dev_attr); |
254 | |
255 | ssize_t cnt, rc = 0; |
256 | |
257 | for (cnt = 0; cnt < s_attr->size; cnt++) { |
258 | if (cnt && !(cnt % 16)) { |
259 | if (PAGE_SIZE - rc) |
260 | buffer[rc++] = '\n'; |
261 | } |
262 | |
263 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "%02x " , |
264 | ((unsigned char *)s_attr->data)[cnt]); |
265 | } |
266 | return rc; |
267 | } |
268 | |
269 | static ssize_t mspro_block_attr_show_sysinfo(struct device *dev, |
270 | struct device_attribute *attr, |
271 | char *buffer) |
272 | { |
273 | struct mspro_sys_attr *x_attr = container_of(attr, |
274 | struct mspro_sys_attr, |
275 | dev_attr); |
276 | struct mspro_sys_info *x_sys = x_attr->data; |
277 | ssize_t rc = 0; |
278 | int date_tz = 0, date_tz_f = 0; |
279 | |
280 | if (x_sys->assembly_date[0] > 0x80U) { |
281 | date_tz = (~x_sys->assembly_date[0]) + 1; |
282 | date_tz_f = date_tz & 3; |
283 | date_tz >>= 2; |
284 | date_tz = -date_tz; |
285 | date_tz_f *= 15; |
286 | } else if (x_sys->assembly_date[0] < 0x80U) { |
287 | date_tz = x_sys->assembly_date[0]; |
288 | date_tz_f = date_tz & 3; |
289 | date_tz >>= 2; |
290 | date_tz_f *= 15; |
291 | } |
292 | |
293 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "class: %x\n" , x_sys->class); |
294 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "block size: %x\n" , be16_to_cpu(x_sys->block_size)); |
295 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "block count: %x\n" , be16_to_cpu(x_sys->block_count)); |
296 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "user block count: %x\n" , |
297 | be16_to_cpu(x_sys->user_block_count)); |
298 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "page size: %x\n" , be16_to_cpu(x_sys->page_size)); |
299 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "assembly date: GMT%+d:%d %04u-%02u-%02u %02u:%02u:%02u\n" , |
300 | date_tz, date_tz_f, |
301 | be16_to_cpup(p: (__be16 *)&x_sys->assembly_date[1]), |
302 | x_sys->assembly_date[3], x_sys->assembly_date[4], |
303 | x_sys->assembly_date[5], x_sys->assembly_date[6], |
304 | x_sys->assembly_date[7]); |
305 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "serial number: %x\n" , be32_to_cpu(x_sys->serial_number)); |
306 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "assembly maker code: %x\n" , x_sys->assembly_maker_code); |
307 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "assembly model code: %02x%02x%02x\n" , |
308 | x_sys->assembly_model_code[0], |
309 | x_sys->assembly_model_code[1], |
310 | x_sys->assembly_model_code[2]); |
311 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "memory maker code: %x\n" , |
312 | be16_to_cpu(x_sys->memory_maker_code)); |
313 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "memory model code: %x\n" , |
314 | be16_to_cpu(x_sys->memory_model_code)); |
315 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "vcc: %x\n" , x_sys->vcc); |
316 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "vpp: %x\n" , x_sys->vpp); |
317 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "controller number: %x\n" , |
318 | be16_to_cpu(x_sys->controller_number)); |
319 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "controller function: %x\n" , |
320 | be16_to_cpu(x_sys->controller_function)); |
321 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "start sector: %x\n" , be16_to_cpu(x_sys->start_sector)); |
322 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "unit size: %x\n" , be16_to_cpu(x_sys->unit_size)); |
323 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "sub class: %x\n" , x_sys->ms_sub_class); |
324 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "interface type: %x\n" , x_sys->interface_type); |
325 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "controller code: %x\n" , |
326 | be16_to_cpu(x_sys->controller_code)); |
327 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "format type: %x\n" , x_sys->format_type); |
328 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "device type: %x\n" , x_sys->device_type); |
329 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "mspro id: %s\n" , x_sys->mspro_id); |
330 | return rc; |
331 | } |
332 | |
333 | static ssize_t mspro_block_attr_show_modelname(struct device *dev, |
334 | struct device_attribute *attr, |
335 | char *buffer) |
336 | { |
337 | struct mspro_sys_attr *s_attr = container_of(attr, |
338 | struct mspro_sys_attr, |
339 | dev_attr); |
340 | |
341 | return sysfs_emit(buf: buffer, fmt: "%s" , (char *)s_attr->data); |
342 | } |
343 | |
344 | static ssize_t mspro_block_attr_show_mbr(struct device *dev, |
345 | struct device_attribute *attr, |
346 | char *buffer) |
347 | { |
348 | struct mspro_sys_attr *x_attr = container_of(attr, |
349 | struct mspro_sys_attr, |
350 | dev_attr); |
351 | struct mspro_mbr *x_mbr = x_attr->data; |
352 | ssize_t rc = 0; |
353 | |
354 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "boot partition: %x\n" , x_mbr->boot_partition); |
355 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "start head: %x\n" , x_mbr->start_head); |
356 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "start sector: %x\n" , x_mbr->start_sector); |
357 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "start cylinder: %x\n" , x_mbr->start_cylinder); |
358 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "partition type: %x\n" , x_mbr->partition_type); |
359 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "end head: %x\n" , x_mbr->end_head); |
360 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "end sector: %x\n" , x_mbr->end_sector); |
361 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "end cylinder: %x\n" , x_mbr->end_cylinder); |
362 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "start sectors: %x\n" , x_mbr->start_sectors); |
363 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "sectors per partition: %x\n" , |
364 | x_mbr->sectors_per_partition); |
365 | return rc; |
366 | } |
367 | |
368 | static ssize_t mspro_block_attr_show_specfile(struct device *dev, |
369 | struct device_attribute *attr, |
370 | char *buffer) |
371 | { |
372 | struct mspro_sys_attr *x_attr = container_of(attr, |
373 | struct mspro_sys_attr, |
374 | dev_attr); |
375 | struct mspro_specfile *x_spfile = x_attr->data; |
376 | char name[9], ext[4]; |
377 | ssize_t rc = 0; |
378 | |
379 | memcpy(name, x_spfile->name, 8); |
380 | name[8] = 0; |
381 | memcpy(ext, x_spfile->ext, 3); |
382 | ext[3] = 0; |
383 | |
384 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "name: %s\n" , name); |
385 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "ext: %s\n" , ext); |
386 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "attribute: %x\n" , x_spfile->attr); |
387 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "time: %d:%d:%d\n" , |
388 | x_spfile->time >> 11, |
389 | (x_spfile->time >> 5) & 0x3f, |
390 | (x_spfile->time & 0x1f) * 2); |
391 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "date: %d-%d-%d\n" , |
392 | (x_spfile->date >> 9) + 1980, |
393 | (x_spfile->date >> 5) & 0xf, |
394 | x_spfile->date & 0x1f); |
395 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "start cluster: %x\n" , x_spfile->cluster); |
396 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "size: %x\n" , x_spfile->size); |
397 | return rc; |
398 | } |
399 | |
400 | static ssize_t mspro_block_attr_show_devinfo(struct device *dev, |
401 | struct device_attribute *attr, |
402 | char *buffer) |
403 | { |
404 | struct mspro_sys_attr *x_attr = container_of(attr, |
405 | struct mspro_sys_attr, |
406 | dev_attr); |
407 | struct mspro_devinfo *x_devinfo = x_attr->data; |
408 | ssize_t rc = 0; |
409 | |
410 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "cylinders: %x\n" , be16_to_cpu(x_devinfo->cylinders)); |
411 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "heads: %x\n" , be16_to_cpu(x_devinfo->heads)); |
412 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "bytes per track: %x\n" , |
413 | be16_to_cpu(x_devinfo->bytes_per_track)); |
414 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "bytes per sector: %x\n" , |
415 | be16_to_cpu(x_devinfo->bytes_per_sector)); |
416 | rc += sysfs_emit_at(buf: buffer, at: rc, fmt: "sectors per track: %x\n" , |
417 | be16_to_cpu(x_devinfo->sectors_per_track)); |
418 | return rc; |
419 | } |
420 | |
421 | static sysfs_show_t mspro_block_attr_show(unsigned char tag) |
422 | { |
423 | switch (tag) { |
424 | case MSPRO_BLOCK_ID_SYSINFO: |
425 | return mspro_block_attr_show_sysinfo; |
426 | case MSPRO_BLOCK_ID_MODELNAME: |
427 | return mspro_block_attr_show_modelname; |
428 | case MSPRO_BLOCK_ID_MBR: |
429 | return mspro_block_attr_show_mbr; |
430 | case MSPRO_BLOCK_ID_SPECFILEVALUES1: |
431 | case MSPRO_BLOCK_ID_SPECFILEVALUES2: |
432 | return mspro_block_attr_show_specfile; |
433 | case MSPRO_BLOCK_ID_DEVINFO: |
434 | return mspro_block_attr_show_devinfo; |
435 | default: |
436 | return mspro_block_attr_show_default; |
437 | } |
438 | } |
439 | |
440 | /*** Protocol handlers ***/ |
441 | |
442 | /* |
443 | * Functions prefixed with "h_" are protocol callbacks. They can be called from |
444 | * interrupt context. Return value of 0 means that request processing is still |
445 | * ongoing, while special error value of -EAGAIN means that current request is |
446 | * finished (and request processor should come back some time later). |
447 | */ |
448 | |
449 | static int h_mspro_block_req_init(struct memstick_dev *card, |
450 | struct memstick_request **mrq) |
451 | { |
452 | struct mspro_block_data *msb = memstick_get_drvdata(card); |
453 | |
454 | *mrq = &card->current_mrq; |
455 | card->next_request = msb->mrq_handler; |
456 | return 0; |
457 | } |
458 | |
459 | static int h_mspro_block_default(struct memstick_dev *card, |
460 | struct memstick_request **mrq) |
461 | { |
462 | return mspro_block_complete_req(card, error: (*mrq)->error); |
463 | } |
464 | |
465 | static int h_mspro_block_default_bad(struct memstick_dev *card, |
466 | struct memstick_request **mrq) |
467 | { |
468 | return -ENXIO; |
469 | } |
470 | |
471 | static int h_mspro_block_get_ro(struct memstick_dev *card, |
472 | struct memstick_request **mrq) |
473 | { |
474 | struct mspro_block_data *msb = memstick_get_drvdata(card); |
475 | |
476 | if (!(*mrq)->error) { |
477 | if ((*mrq)->data[offsetof(struct ms_status_register, status0)] |
478 | & MEMSTICK_STATUS0_WP) |
479 | msb->read_only = 1; |
480 | else |
481 | msb->read_only = 0; |
482 | } |
483 | |
484 | return mspro_block_complete_req(card, error: (*mrq)->error); |
485 | } |
486 | |
487 | static int h_mspro_block_wait_for_ced(struct memstick_dev *card, |
488 | struct memstick_request **mrq) |
489 | { |
490 | dev_dbg(&card->dev, "wait for ced: value %x\n" , (*mrq)->data[0]); |
491 | |
492 | if (!(*mrq)->error) { |
493 | if ((*mrq)->data[0] & (MEMSTICK_INT_CMDNAK | MEMSTICK_INT_ERR)) |
494 | (*mrq)->error = -EFAULT; |
495 | else if (!((*mrq)->data[0] & MEMSTICK_INT_CED)) |
496 | return 0; |
497 | } |
498 | |
499 | return mspro_block_complete_req(card, error: (*mrq)->error); |
500 | } |
501 | |
502 | static int h_mspro_block_transfer_data(struct memstick_dev *card, |
503 | struct memstick_request **mrq) |
504 | { |
505 | struct mspro_block_data *msb = memstick_get_drvdata(card); |
506 | unsigned char t_val = 0; |
507 | struct scatterlist t_sg = { 0 }; |
508 | size_t t_offset; |
509 | |
510 | if ((*mrq)->error) |
511 | return mspro_block_complete_req(card, error: (*mrq)->error); |
512 | |
513 | switch ((*mrq)->tpc) { |
514 | case MS_TPC_WRITE_REG: |
515 | memstick_init_req(mrq: *mrq, tpc: MS_TPC_SET_CMD, buf: &msb->transfer_cmd, length: 1); |
516 | (*mrq)->need_card_int = 1; |
517 | return 0; |
518 | case MS_TPC_SET_CMD: |
519 | t_val = (*mrq)->int_reg; |
520 | memstick_init_req(mrq: *mrq, tpc: MS_TPC_GET_INT, NULL, length: 1); |
521 | if (msb->caps & MEMSTICK_CAP_AUTO_GET_INT) |
522 | goto has_int_reg; |
523 | return 0; |
524 | case MS_TPC_GET_INT: |
525 | t_val = (*mrq)->data[0]; |
526 | has_int_reg: |
527 | if (t_val & (MEMSTICK_INT_CMDNAK | MEMSTICK_INT_ERR)) { |
528 | t_val = MSPRO_CMD_STOP; |
529 | memstick_init_req(mrq: *mrq, tpc: MS_TPC_SET_CMD, buf: &t_val, length: 1); |
530 | card->next_request = h_mspro_block_default; |
531 | return 0; |
532 | } |
533 | |
534 | if (msb->current_page |
535 | == (msb->req_sg[msb->current_seg].length |
536 | / msb->page_size)) { |
537 | msb->current_page = 0; |
538 | msb->current_seg++; |
539 | |
540 | if (msb->current_seg == msb->seg_count) { |
541 | if (t_val & MEMSTICK_INT_CED) { |
542 | return mspro_block_complete_req(card, |
543 | error: 0); |
544 | } else { |
545 | card->next_request |
546 | = h_mspro_block_wait_for_ced; |
547 | memstick_init_req(mrq: *mrq, tpc: MS_TPC_GET_INT, |
548 | NULL, length: 1); |
549 | return 0; |
550 | } |
551 | } |
552 | } |
553 | |
554 | if (!(t_val & MEMSTICK_INT_BREQ)) { |
555 | memstick_init_req(mrq: *mrq, tpc: MS_TPC_GET_INT, NULL, length: 1); |
556 | return 0; |
557 | } |
558 | |
559 | t_offset = msb->req_sg[msb->current_seg].offset; |
560 | t_offset += msb->current_page * msb->page_size; |
561 | |
562 | sg_set_page(sg: &t_sg, |
563 | nth_page(sg_page(&(msb->req_sg[msb->current_seg])), |
564 | t_offset >> PAGE_SHIFT), |
565 | len: msb->page_size, offset_in_page(t_offset)); |
566 | |
567 | memstick_init_req_sg(mrq: *mrq, tpc: msb->data_dir == READ |
568 | ? MS_TPC_READ_LONG_DATA |
569 | : MS_TPC_WRITE_LONG_DATA, |
570 | sg: &t_sg); |
571 | (*mrq)->need_card_int = 1; |
572 | return 0; |
573 | case MS_TPC_READ_LONG_DATA: |
574 | case MS_TPC_WRITE_LONG_DATA: |
575 | msb->current_page++; |
576 | if (msb->caps & MEMSTICK_CAP_AUTO_GET_INT) { |
577 | t_val = (*mrq)->int_reg; |
578 | goto has_int_reg; |
579 | } else { |
580 | memstick_init_req(mrq: *mrq, tpc: MS_TPC_GET_INT, NULL, length: 1); |
581 | return 0; |
582 | } |
583 | |
584 | default: |
585 | BUG(); |
586 | } |
587 | } |
588 | |
589 | /*** Transfer setup functions for different access methods. ***/ |
590 | |
591 | /** Setup data transfer request for SET_CMD TPC with arguments in card |
592 | * registers. |
593 | * |
594 | * @card Current media instance |
595 | * @offset Target data offset in bytes |
596 | * @length Required transfer length in bytes. |
597 | */ |
598 | static void h_mspro_block_setup_cmd(struct memstick_dev *card, u64 offset, |
599 | size_t length) |
600 | { |
601 | struct mspro_block_data *msb = memstick_get_drvdata(card); |
602 | struct mspro_param_register param = { |
603 | .system = msb->system, |
604 | .data_count = cpu_to_be16((uint16_t)(length / msb->page_size)), |
605 | /* ISO C90 warning precludes direct initialization for now. */ |
606 | .data_address = 0, |
607 | .tpc_param = 0 |
608 | }; |
609 | |
610 | do_div(offset, msb->page_size); |
611 | param.data_address = cpu_to_be32((uint32_t)offset); |
612 | |
613 | card->next_request = h_mspro_block_req_init; |
614 | msb->mrq_handler = h_mspro_block_transfer_data; |
615 | memstick_init_req(mrq: &card->current_mrq, tpc: MS_TPC_WRITE_REG, |
616 | buf: ¶m, length: sizeof(param)); |
617 | } |
618 | |
619 | /*** Data transfer ***/ |
620 | |
621 | static int mspro_block_issue_req(struct memstick_dev *card) |
622 | { |
623 | struct mspro_block_data *msb = memstick_get_drvdata(card); |
624 | u64 t_off; |
625 | unsigned int count; |
626 | |
627 | while (true) { |
628 | msb->current_page = 0; |
629 | msb->current_seg = 0; |
630 | msb->seg_count = blk_rq_map_sg(q: msb->block_req->q, |
631 | rq: msb->block_req, |
632 | sglist: msb->req_sg); |
633 | |
634 | if (!msb->seg_count) { |
635 | unsigned int bytes = blk_rq_cur_bytes(rq: msb->block_req); |
636 | bool chunk; |
637 | |
638 | chunk = blk_update_request(rq: msb->block_req, |
639 | BLK_STS_RESOURCE, |
640 | nr_bytes: bytes); |
641 | if (chunk) |
642 | continue; |
643 | __blk_mq_end_request(rq: msb->block_req, |
644 | BLK_STS_RESOURCE); |
645 | msb->block_req = NULL; |
646 | return -EAGAIN; |
647 | } |
648 | |
649 | t_off = blk_rq_pos(rq: msb->block_req); |
650 | t_off <<= 9; |
651 | count = blk_rq_bytes(rq: msb->block_req); |
652 | |
653 | msb->setup_transfer(card, t_off, count); |
654 | |
655 | msb->data_dir = rq_data_dir(msb->block_req); |
656 | msb->transfer_cmd = msb->data_dir == READ |
657 | ? MSPRO_CMD_READ_DATA |
658 | : MSPRO_CMD_WRITE_DATA; |
659 | |
660 | memstick_new_req(host: card->host); |
661 | return 0; |
662 | } |
663 | } |
664 | |
665 | static int mspro_block_complete_req(struct memstick_dev *card, int error) |
666 | { |
667 | struct mspro_block_data *msb = memstick_get_drvdata(card); |
668 | int cnt; |
669 | bool chunk; |
670 | unsigned int t_len = 0; |
671 | unsigned long flags; |
672 | |
673 | spin_lock_irqsave(&msb->q_lock, flags); |
674 | dev_dbg(&card->dev, "complete %d, %d\n" , msb->block_req ? 1 : 0, |
675 | error); |
676 | |
677 | if (msb->block_req) { |
678 | /* Nothing to do - not really an error */ |
679 | if (error == -EAGAIN) |
680 | error = 0; |
681 | |
682 | if (error || (card->current_mrq.tpc == MSPRO_CMD_STOP)) { |
683 | if (msb->data_dir == READ) { |
684 | for (cnt = 0; cnt < msb->current_seg; cnt++) { |
685 | t_len += msb->req_sg[cnt].length |
686 | / msb->page_size; |
687 | |
688 | if (msb->current_page) |
689 | t_len += msb->current_page - 1; |
690 | |
691 | t_len *= msb->page_size; |
692 | } |
693 | } |
694 | } else |
695 | t_len = blk_rq_bytes(rq: msb->block_req); |
696 | |
697 | dev_dbg(&card->dev, "transferred %x (%d)\n" , t_len, error); |
698 | |
699 | if (error && !t_len) |
700 | t_len = blk_rq_cur_bytes(rq: msb->block_req); |
701 | |
702 | chunk = blk_update_request(rq: msb->block_req, |
703 | error: errno_to_blk_status(errno: error), nr_bytes: t_len); |
704 | if (chunk) { |
705 | error = mspro_block_issue_req(card); |
706 | if (!error) |
707 | goto out; |
708 | } else { |
709 | __blk_mq_end_request(rq: msb->block_req, |
710 | error: errno_to_blk_status(errno: error)); |
711 | msb->block_req = NULL; |
712 | } |
713 | } else { |
714 | if (!error) |
715 | error = -EAGAIN; |
716 | } |
717 | |
718 | card->next_request = h_mspro_block_default_bad; |
719 | complete_all(&card->mrq_complete); |
720 | out: |
721 | spin_unlock_irqrestore(lock: &msb->q_lock, flags); |
722 | return error; |
723 | } |
724 | |
725 | static void mspro_block_stop(struct memstick_dev *card) |
726 | { |
727 | struct mspro_block_data *msb = memstick_get_drvdata(card); |
728 | int rc = 0; |
729 | unsigned long flags; |
730 | |
731 | while (1) { |
732 | spin_lock_irqsave(&msb->q_lock, flags); |
733 | if (!msb->block_req) { |
734 | blk_mq_stop_hw_queues(q: msb->queue); |
735 | rc = 1; |
736 | } |
737 | spin_unlock_irqrestore(lock: &msb->q_lock, flags); |
738 | |
739 | if (rc) |
740 | break; |
741 | |
742 | wait_for_completion(&card->mrq_complete); |
743 | } |
744 | } |
745 | |
746 | static void mspro_block_start(struct memstick_dev *card) |
747 | { |
748 | struct mspro_block_data *msb = memstick_get_drvdata(card); |
749 | |
750 | blk_mq_start_hw_queues(q: msb->queue); |
751 | } |
752 | |
753 | static blk_status_t mspro_queue_rq(struct blk_mq_hw_ctx *hctx, |
754 | const struct blk_mq_queue_data *bd) |
755 | { |
756 | struct memstick_dev *card = hctx->queue->queuedata; |
757 | struct mspro_block_data *msb = memstick_get_drvdata(card); |
758 | |
759 | spin_lock_irq(lock: &msb->q_lock); |
760 | |
761 | if (msb->block_req) { |
762 | spin_unlock_irq(lock: &msb->q_lock); |
763 | return BLK_STS_DEV_RESOURCE; |
764 | } |
765 | |
766 | if (msb->eject) { |
767 | spin_unlock_irq(lock: &msb->q_lock); |
768 | blk_mq_start_request(rq: bd->rq); |
769 | return BLK_STS_IOERR; |
770 | } |
771 | |
772 | msb->block_req = bd->rq; |
773 | blk_mq_start_request(rq: bd->rq); |
774 | |
775 | if (mspro_block_issue_req(card)) |
776 | msb->block_req = NULL; |
777 | |
778 | spin_unlock_irq(lock: &msb->q_lock); |
779 | return BLK_STS_OK; |
780 | } |
781 | |
782 | /*** Initialization ***/ |
783 | |
784 | static int mspro_block_wait_for_ced(struct memstick_dev *card) |
785 | { |
786 | struct mspro_block_data *msb = memstick_get_drvdata(card); |
787 | |
788 | card->next_request = h_mspro_block_req_init; |
789 | msb->mrq_handler = h_mspro_block_wait_for_ced; |
790 | memstick_init_req(mrq: &card->current_mrq, tpc: MS_TPC_GET_INT, NULL, length: 1); |
791 | memstick_new_req(host: card->host); |
792 | wait_for_completion(&card->mrq_complete); |
793 | return card->current_mrq.error; |
794 | } |
795 | |
796 | static int mspro_block_set_interface(struct memstick_dev *card, |
797 | unsigned char sys_reg) |
798 | { |
799 | struct memstick_host *host = card->host; |
800 | struct mspro_block_data *msb = memstick_get_drvdata(card); |
801 | struct mspro_param_register param = { |
802 | .system = sys_reg, |
803 | .data_count = 0, |
804 | .data_address = 0, |
805 | .tpc_param = 0 |
806 | }; |
807 | |
808 | card->next_request = h_mspro_block_req_init; |
809 | msb->mrq_handler = h_mspro_block_default; |
810 | memstick_init_req(mrq: &card->current_mrq, tpc: MS_TPC_WRITE_REG, buf: ¶m, |
811 | length: sizeof(param)); |
812 | memstick_new_req(host); |
813 | wait_for_completion(&card->mrq_complete); |
814 | return card->current_mrq.error; |
815 | } |
816 | |
817 | static int mspro_block_switch_interface(struct memstick_dev *card) |
818 | { |
819 | struct memstick_host *host = card->host; |
820 | struct mspro_block_data *msb = memstick_get_drvdata(card); |
821 | int rc = 0; |
822 | |
823 | try_again: |
824 | if (msb->caps & MEMSTICK_CAP_PAR4) |
825 | rc = mspro_block_set_interface(card, MEMSTICK_SYS_PAR4); |
826 | else |
827 | return 0; |
828 | |
829 | if (rc) { |
830 | printk(KERN_WARNING |
831 | "%s: could not switch to 4-bit mode, error %d\n" , |
832 | dev_name(&card->dev), rc); |
833 | return 0; |
834 | } |
835 | |
836 | msb->system = MEMSTICK_SYS_PAR4; |
837 | host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_PAR4); |
838 | printk(KERN_INFO "%s: switching to 4-bit parallel mode\n" , |
839 | dev_name(&card->dev)); |
840 | |
841 | if (msb->caps & MEMSTICK_CAP_PAR8) { |
842 | rc = mspro_block_set_interface(card, MEMSTICK_SYS_PAR8); |
843 | |
844 | if (!rc) { |
845 | msb->system = MEMSTICK_SYS_PAR8; |
846 | host->set_param(host, MEMSTICK_INTERFACE, |
847 | MEMSTICK_PAR8); |
848 | printk(KERN_INFO |
849 | "%s: switching to 8-bit parallel mode\n" , |
850 | dev_name(&card->dev)); |
851 | } else |
852 | printk(KERN_WARNING |
853 | "%s: could not switch to 8-bit mode, error %d\n" , |
854 | dev_name(&card->dev), rc); |
855 | } |
856 | |
857 | card->next_request = h_mspro_block_req_init; |
858 | msb->mrq_handler = h_mspro_block_default; |
859 | memstick_init_req(mrq: &card->current_mrq, tpc: MS_TPC_GET_INT, NULL, length: 1); |
860 | memstick_new_req(host: card->host); |
861 | wait_for_completion(&card->mrq_complete); |
862 | rc = card->current_mrq.error; |
863 | |
864 | if (rc) { |
865 | printk(KERN_WARNING |
866 | "%s: interface error, trying to fall back to serial\n" , |
867 | dev_name(&card->dev)); |
868 | msb->system = MEMSTICK_SYS_SERIAL; |
869 | host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_OFF); |
870 | msleep(msecs: 10); |
871 | host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_ON); |
872 | host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_SERIAL); |
873 | |
874 | rc = memstick_set_rw_addr(card); |
875 | if (!rc) |
876 | rc = mspro_block_set_interface(card, sys_reg: msb->system); |
877 | |
878 | if (!rc) { |
879 | msleep(msecs: 150); |
880 | rc = mspro_block_wait_for_ced(card); |
881 | if (rc) |
882 | return rc; |
883 | |
884 | if (msb->caps & MEMSTICK_CAP_PAR8) { |
885 | msb->caps &= ~MEMSTICK_CAP_PAR8; |
886 | goto try_again; |
887 | } |
888 | } |
889 | } |
890 | return rc; |
891 | } |
892 | |
893 | /* Memory allocated for attributes by this function should be freed by |
894 | * mspro_block_data_clear, no matter if the initialization process succeeded |
895 | * or failed. |
896 | */ |
897 | static int mspro_block_read_attributes(struct memstick_dev *card) |
898 | { |
899 | struct mspro_block_data *msb = memstick_get_drvdata(card); |
900 | struct mspro_attribute *attr = NULL; |
901 | struct mspro_sys_attr *s_attr = NULL; |
902 | unsigned char *buffer = NULL; |
903 | int cnt, rc, attr_count; |
904 | /* While normally physical device offsets, represented here by |
905 | * attr_offset and attr_len will be of large numeric types, we can be |
906 | * sure, that attributes are close enough to the beginning of the |
907 | * device, to save ourselves some trouble. |
908 | */ |
909 | unsigned int addr, attr_offset = 0, attr_len = msb->page_size; |
910 | |
911 | attr = kmalloc(size: msb->page_size, GFP_KERNEL); |
912 | if (!attr) |
913 | return -ENOMEM; |
914 | |
915 | sg_init_one(&msb->req_sg[0], attr, msb->page_size); |
916 | msb->seg_count = 1; |
917 | msb->current_seg = 0; |
918 | msb->current_page = 0; |
919 | msb->data_dir = READ; |
920 | msb->transfer_cmd = MSPRO_CMD_READ_ATRB; |
921 | |
922 | msb->setup_transfer(card, attr_offset, attr_len); |
923 | |
924 | memstick_new_req(host: card->host); |
925 | wait_for_completion(&card->mrq_complete); |
926 | if (card->current_mrq.error) { |
927 | rc = card->current_mrq.error; |
928 | goto out_free_attr; |
929 | } |
930 | |
931 | if (be16_to_cpu(attr->signature) != MSPRO_BLOCK_SIGNATURE) { |
932 | printk(KERN_ERR "%s: unrecognized device signature %x\n" , |
933 | dev_name(&card->dev), be16_to_cpu(attr->signature)); |
934 | rc = -ENODEV; |
935 | goto out_free_attr; |
936 | } |
937 | |
938 | if (attr->count > MSPRO_BLOCK_MAX_ATTRIBUTES) { |
939 | printk(KERN_WARNING "%s: way too many attribute entries\n" , |
940 | dev_name(&card->dev)); |
941 | attr_count = MSPRO_BLOCK_MAX_ATTRIBUTES; |
942 | } else |
943 | attr_count = attr->count; |
944 | |
945 | msb->attr_group.attrs = kcalloc(n: attr_count + 1, |
946 | size: sizeof(*msb->attr_group.attrs), |
947 | GFP_KERNEL); |
948 | if (!msb->attr_group.attrs) { |
949 | rc = -ENOMEM; |
950 | goto out_free_attr; |
951 | } |
952 | msb->attr_group.name = "media_attributes" ; |
953 | |
954 | buffer = kmemdup(p: attr, size: attr_len, GFP_KERNEL); |
955 | if (!buffer) { |
956 | rc = -ENOMEM; |
957 | goto out_free_attr; |
958 | } |
959 | |
960 | for (cnt = 0; cnt < attr_count; ++cnt) { |
961 | s_attr = kzalloc(size: sizeof(struct mspro_sys_attr), GFP_KERNEL); |
962 | if (!s_attr) { |
963 | rc = -ENOMEM; |
964 | goto out_free_buffer; |
965 | } |
966 | |
967 | msb->attr_group.attrs[cnt] = &s_attr->dev_attr.attr; |
968 | addr = be32_to_cpu(attr->entries[cnt].address); |
969 | s_attr->size = be32_to_cpu(attr->entries[cnt].size); |
970 | dev_dbg(&card->dev, "adding attribute %d: id %x, address %x, " |
971 | "size %zx\n" , cnt, attr->entries[cnt].id, addr, |
972 | s_attr->size); |
973 | s_attr->id = attr->entries[cnt].id; |
974 | if (mspro_block_attr_name(tag: s_attr->id)) |
975 | snprintf(buf: s_attr->name, size: sizeof(s_attr->name), fmt: "%s" , |
976 | mspro_block_attr_name(tag: attr->entries[cnt].id)); |
977 | else |
978 | snprintf(buf: s_attr->name, size: sizeof(s_attr->name), |
979 | fmt: "attr_x%02x" , attr->entries[cnt].id); |
980 | |
981 | sysfs_attr_init(&s_attr->dev_attr.attr); |
982 | s_attr->dev_attr.attr.name = s_attr->name; |
983 | s_attr->dev_attr.attr.mode = S_IRUGO; |
984 | s_attr->dev_attr.show = mspro_block_attr_show(tag: s_attr->id); |
985 | |
986 | if (!s_attr->size) |
987 | continue; |
988 | |
989 | s_attr->data = kmalloc(size: s_attr->size, GFP_KERNEL); |
990 | if (!s_attr->data) { |
991 | rc = -ENOMEM; |
992 | goto out_free_buffer; |
993 | } |
994 | |
995 | if (((addr / msb->page_size) == (attr_offset / msb->page_size)) |
996 | && (((addr + s_attr->size - 1) / msb->page_size) |
997 | == (attr_offset / msb->page_size))) { |
998 | memcpy(s_attr->data, buffer + addr % msb->page_size, |
999 | s_attr->size); |
1000 | continue; |
1001 | } |
1002 | |
1003 | attr_offset = (addr / msb->page_size) * msb->page_size; |
1004 | |
1005 | if ((attr_offset + attr_len) < (addr + s_attr->size)) { |
1006 | kfree(objp: buffer); |
1007 | attr_len = (((addr + s_attr->size) / msb->page_size) |
1008 | + 1 ) * msb->page_size - attr_offset; |
1009 | buffer = kmalloc(size: attr_len, GFP_KERNEL); |
1010 | if (!buffer) { |
1011 | rc = -ENOMEM; |
1012 | goto out_free_attr; |
1013 | } |
1014 | } |
1015 | |
1016 | sg_init_one(&msb->req_sg[0], buffer, attr_len); |
1017 | msb->seg_count = 1; |
1018 | msb->current_seg = 0; |
1019 | msb->current_page = 0; |
1020 | msb->data_dir = READ; |
1021 | msb->transfer_cmd = MSPRO_CMD_READ_ATRB; |
1022 | |
1023 | dev_dbg(&card->dev, "reading attribute range %x, %x\n" , |
1024 | attr_offset, attr_len); |
1025 | |
1026 | msb->setup_transfer(card, attr_offset, attr_len); |
1027 | memstick_new_req(host: card->host); |
1028 | wait_for_completion(&card->mrq_complete); |
1029 | if (card->current_mrq.error) { |
1030 | rc = card->current_mrq.error; |
1031 | goto out_free_buffer; |
1032 | } |
1033 | |
1034 | memcpy(s_attr->data, buffer + addr % msb->page_size, |
1035 | s_attr->size); |
1036 | } |
1037 | |
1038 | rc = 0; |
1039 | out_free_buffer: |
1040 | kfree(objp: buffer); |
1041 | out_free_attr: |
1042 | kfree(objp: attr); |
1043 | return rc; |
1044 | } |
1045 | |
1046 | static int mspro_block_init_card(struct memstick_dev *card) |
1047 | { |
1048 | struct mspro_block_data *msb = memstick_get_drvdata(card); |
1049 | struct memstick_host *host = card->host; |
1050 | int rc = 0; |
1051 | |
1052 | msb->system = MEMSTICK_SYS_SERIAL; |
1053 | msb->setup_transfer = h_mspro_block_setup_cmd; |
1054 | |
1055 | card->reg_addr.r_offset = offsetof(struct mspro_register, status); |
1056 | card->reg_addr.r_length = sizeof(struct ms_status_register); |
1057 | card->reg_addr.w_offset = offsetof(struct mspro_register, param); |
1058 | card->reg_addr.w_length = sizeof(struct mspro_param_register); |
1059 | |
1060 | if (memstick_set_rw_addr(card)) |
1061 | return -EIO; |
1062 | |
1063 | msb->caps = host->caps; |
1064 | |
1065 | msleep(msecs: 150); |
1066 | rc = mspro_block_wait_for_ced(card); |
1067 | if (rc) |
1068 | return rc; |
1069 | |
1070 | rc = mspro_block_switch_interface(card); |
1071 | if (rc) |
1072 | return rc; |
1073 | |
1074 | dev_dbg(&card->dev, "card activated\n" ); |
1075 | if (msb->system != MEMSTICK_SYS_SERIAL) |
1076 | msb->caps |= MEMSTICK_CAP_AUTO_GET_INT; |
1077 | |
1078 | card->next_request = h_mspro_block_req_init; |
1079 | msb->mrq_handler = h_mspro_block_get_ro; |
1080 | memstick_init_req(mrq: &card->current_mrq, tpc: MS_TPC_READ_REG, NULL, |
1081 | length: sizeof(struct ms_status_register)); |
1082 | memstick_new_req(host: card->host); |
1083 | wait_for_completion(&card->mrq_complete); |
1084 | if (card->current_mrq.error) |
1085 | return card->current_mrq.error; |
1086 | |
1087 | dev_dbg(&card->dev, "card r/w status %d\n" , msb->read_only ? 0 : 1); |
1088 | |
1089 | msb->page_size = 512; |
1090 | rc = mspro_block_read_attributes(card); |
1091 | if (rc) |
1092 | return rc; |
1093 | |
1094 | dev_dbg(&card->dev, "attributes loaded\n" ); |
1095 | return 0; |
1096 | |
1097 | } |
1098 | |
1099 | static const struct blk_mq_ops mspro_mq_ops = { |
1100 | .queue_rq = mspro_queue_rq, |
1101 | }; |
1102 | |
1103 | static int mspro_block_init_disk(struct memstick_dev *card) |
1104 | { |
1105 | struct mspro_block_data *msb = memstick_get_drvdata(card); |
1106 | struct queue_limits lim = { |
1107 | .logical_block_size = msb->page_size, |
1108 | .max_hw_sectors = MSPRO_BLOCK_MAX_PAGES, |
1109 | .max_segments = MSPRO_BLOCK_MAX_SEGS, |
1110 | .max_segment_size = MSPRO_BLOCK_MAX_PAGES * msb->page_size, |
1111 | }; |
1112 | struct mspro_devinfo *dev_info = NULL; |
1113 | struct mspro_sys_info *sys_info = NULL; |
1114 | struct mspro_sys_attr *s_attr = NULL; |
1115 | int rc, disk_id; |
1116 | unsigned long capacity; |
1117 | |
1118 | for (rc = 0; msb->attr_group.attrs[rc]; ++rc) { |
1119 | s_attr = mspro_from_sysfs_attr(attr: msb->attr_group.attrs[rc]); |
1120 | |
1121 | if (s_attr->id == MSPRO_BLOCK_ID_DEVINFO) |
1122 | dev_info = s_attr->data; |
1123 | else if (s_attr->id == MSPRO_BLOCK_ID_SYSINFO) |
1124 | sys_info = s_attr->data; |
1125 | } |
1126 | |
1127 | if (!dev_info || !sys_info) |
1128 | return -ENODEV; |
1129 | |
1130 | msb->cylinders = be16_to_cpu(dev_info->cylinders); |
1131 | msb->heads = be16_to_cpu(dev_info->heads); |
1132 | msb->sectors_per_track = be16_to_cpu(dev_info->sectors_per_track); |
1133 | |
1134 | msb->page_size = be16_to_cpu(sys_info->unit_size); |
1135 | |
1136 | mutex_lock(&mspro_block_disk_lock); |
1137 | disk_id = idr_alloc(&mspro_block_disk_idr, ptr: card, start: 0, end: 256, GFP_KERNEL); |
1138 | mutex_unlock(lock: &mspro_block_disk_lock); |
1139 | if (disk_id < 0) |
1140 | return disk_id; |
1141 | |
1142 | rc = blk_mq_alloc_sq_tag_set(set: &msb->tag_set, ops: &mspro_mq_ops, queue_depth: 2, |
1143 | set_flags: BLK_MQ_F_SHOULD_MERGE); |
1144 | if (rc) |
1145 | goto out_release_id; |
1146 | |
1147 | msb->disk = blk_mq_alloc_disk(&msb->tag_set, &lim, card); |
1148 | if (IS_ERR(ptr: msb->disk)) { |
1149 | rc = PTR_ERR(ptr: msb->disk); |
1150 | goto out_free_tag_set; |
1151 | } |
1152 | msb->queue = msb->disk->queue; |
1153 | |
1154 | msb->disk->major = major; |
1155 | msb->disk->first_minor = disk_id << MSPRO_BLOCK_PART_SHIFT; |
1156 | msb->disk->minors = 1 << MSPRO_BLOCK_PART_SHIFT; |
1157 | msb->disk->fops = &ms_block_bdops; |
1158 | msb->disk->private_data = msb; |
1159 | |
1160 | sprintf(buf: msb->disk->disk_name, fmt: "mspblk%d" , disk_id); |
1161 | |
1162 | capacity = be16_to_cpu(sys_info->user_block_count); |
1163 | capacity *= be16_to_cpu(sys_info->block_size); |
1164 | capacity *= msb->page_size >> 9; |
1165 | set_capacity(disk: msb->disk, size: capacity); |
1166 | dev_dbg(&card->dev, "capacity set %ld\n" , capacity); |
1167 | |
1168 | if (msb->read_only) |
1169 | set_disk_ro(disk: msb->disk, read_only: true); |
1170 | |
1171 | rc = device_add_disk(parent: &card->dev, disk: msb->disk, NULL); |
1172 | if (rc) |
1173 | goto out_cleanup_disk; |
1174 | msb->active = 1; |
1175 | return 0; |
1176 | |
1177 | out_cleanup_disk: |
1178 | put_disk(disk: msb->disk); |
1179 | out_free_tag_set: |
1180 | blk_mq_free_tag_set(set: &msb->tag_set); |
1181 | out_release_id: |
1182 | mutex_lock(&mspro_block_disk_lock); |
1183 | idr_remove(&mspro_block_disk_idr, id: disk_id); |
1184 | mutex_unlock(lock: &mspro_block_disk_lock); |
1185 | return rc; |
1186 | } |
1187 | |
1188 | static void mspro_block_data_clear(struct mspro_block_data *msb) |
1189 | { |
1190 | int cnt; |
1191 | struct mspro_sys_attr *s_attr; |
1192 | |
1193 | if (msb->attr_group.attrs) { |
1194 | for (cnt = 0; msb->attr_group.attrs[cnt]; ++cnt) { |
1195 | s_attr = mspro_from_sysfs_attr(attr: msb->attr_group |
1196 | .attrs[cnt]); |
1197 | kfree(objp: s_attr->data); |
1198 | kfree(objp: s_attr); |
1199 | } |
1200 | kfree(objp: msb->attr_group.attrs); |
1201 | } |
1202 | |
1203 | msb->card = NULL; |
1204 | } |
1205 | |
1206 | static int mspro_block_check_card(struct memstick_dev *card) |
1207 | { |
1208 | struct mspro_block_data *msb = memstick_get_drvdata(card); |
1209 | |
1210 | return (msb->active == 1); |
1211 | } |
1212 | |
1213 | static int mspro_block_probe(struct memstick_dev *card) |
1214 | { |
1215 | struct mspro_block_data *msb; |
1216 | int rc = 0; |
1217 | |
1218 | msb = kzalloc(size: sizeof(struct mspro_block_data), GFP_KERNEL); |
1219 | if (!msb) |
1220 | return -ENOMEM; |
1221 | memstick_set_drvdata(card, data: msb); |
1222 | msb->card = card; |
1223 | spin_lock_init(&msb->q_lock); |
1224 | |
1225 | rc = mspro_block_init_card(card); |
1226 | |
1227 | if (rc) |
1228 | goto out_free; |
1229 | |
1230 | rc = sysfs_create_group(kobj: &card->dev.kobj, grp: &msb->attr_group); |
1231 | if (rc) |
1232 | goto out_free; |
1233 | |
1234 | rc = mspro_block_init_disk(card); |
1235 | if (!rc) { |
1236 | card->check = mspro_block_check_card; |
1237 | card->stop = mspro_block_stop; |
1238 | card->start = mspro_block_start; |
1239 | return 0; |
1240 | } |
1241 | |
1242 | sysfs_remove_group(kobj: &card->dev.kobj, grp: &msb->attr_group); |
1243 | out_free: |
1244 | memstick_set_drvdata(card, NULL); |
1245 | mspro_block_data_clear(msb); |
1246 | kfree(objp: msb); |
1247 | return rc; |
1248 | } |
1249 | |
1250 | static void mspro_block_remove(struct memstick_dev *card) |
1251 | { |
1252 | struct mspro_block_data *msb = memstick_get_drvdata(card); |
1253 | unsigned long flags; |
1254 | |
1255 | spin_lock_irqsave(&msb->q_lock, flags); |
1256 | msb->eject = 1; |
1257 | spin_unlock_irqrestore(lock: &msb->q_lock, flags); |
1258 | blk_mq_start_hw_queues(q: msb->queue); |
1259 | |
1260 | del_gendisk(gp: msb->disk); |
1261 | dev_dbg(&card->dev, "mspro block remove\n" ); |
1262 | |
1263 | blk_mq_free_tag_set(set: &msb->tag_set); |
1264 | msb->queue = NULL; |
1265 | |
1266 | sysfs_remove_group(kobj: &card->dev.kobj, grp: &msb->attr_group); |
1267 | |
1268 | mutex_lock(&mspro_block_disk_lock); |
1269 | mspro_block_data_clear(msb); |
1270 | mutex_unlock(lock: &mspro_block_disk_lock); |
1271 | |
1272 | put_disk(disk: msb->disk); |
1273 | memstick_set_drvdata(card, NULL); |
1274 | } |
1275 | |
1276 | #ifdef CONFIG_PM |
1277 | |
1278 | static int mspro_block_suspend(struct memstick_dev *card, pm_message_t state) |
1279 | { |
1280 | struct mspro_block_data *msb = memstick_get_drvdata(card); |
1281 | unsigned long flags; |
1282 | |
1283 | blk_mq_stop_hw_queues(q: msb->queue); |
1284 | |
1285 | spin_lock_irqsave(&msb->q_lock, flags); |
1286 | msb->active = 0; |
1287 | spin_unlock_irqrestore(lock: &msb->q_lock, flags); |
1288 | |
1289 | return 0; |
1290 | } |
1291 | |
1292 | static int mspro_block_resume(struct memstick_dev *card) |
1293 | { |
1294 | struct mspro_block_data *msb = memstick_get_drvdata(card); |
1295 | int rc = 0; |
1296 | |
1297 | #ifdef CONFIG_MEMSTICK_UNSAFE_RESUME |
1298 | |
1299 | struct mspro_block_data *new_msb; |
1300 | struct memstick_host *host = card->host; |
1301 | struct mspro_sys_attr *s_attr, *r_attr; |
1302 | unsigned char cnt; |
1303 | |
1304 | mutex_lock(&host->lock); |
1305 | new_msb = kzalloc(size: sizeof(struct mspro_block_data), GFP_KERNEL); |
1306 | if (!new_msb) { |
1307 | rc = -ENOMEM; |
1308 | goto out_unlock; |
1309 | } |
1310 | |
1311 | new_msb->card = card; |
1312 | memstick_set_drvdata(card, data: new_msb); |
1313 | rc = mspro_block_init_card(card); |
1314 | if (rc) |
1315 | goto out_free; |
1316 | |
1317 | for (cnt = 0; new_msb->attr_group.attrs[cnt] |
1318 | && msb->attr_group.attrs[cnt]; ++cnt) { |
1319 | s_attr = mspro_from_sysfs_attr(attr: new_msb->attr_group.attrs[cnt]); |
1320 | r_attr = mspro_from_sysfs_attr(attr: msb->attr_group.attrs[cnt]); |
1321 | |
1322 | if (s_attr->id == MSPRO_BLOCK_ID_SYSINFO |
1323 | && r_attr->id == s_attr->id) { |
1324 | if (memcmp(p: s_attr->data, q: r_attr->data, size: s_attr->size)) |
1325 | break; |
1326 | |
1327 | msb->active = 1; |
1328 | break; |
1329 | } |
1330 | } |
1331 | |
1332 | out_free: |
1333 | memstick_set_drvdata(card, data: msb); |
1334 | mspro_block_data_clear(msb: new_msb); |
1335 | kfree(objp: new_msb); |
1336 | out_unlock: |
1337 | mutex_unlock(lock: &host->lock); |
1338 | |
1339 | #endif /* CONFIG_MEMSTICK_UNSAFE_RESUME */ |
1340 | |
1341 | blk_mq_start_hw_queues(q: msb->queue); |
1342 | return rc; |
1343 | } |
1344 | |
1345 | #else |
1346 | |
1347 | #define mspro_block_suspend NULL |
1348 | #define mspro_block_resume NULL |
1349 | |
1350 | #endif /* CONFIG_PM */ |
1351 | |
1352 | static struct memstick_device_id mspro_block_id_tbl[] = { |
1353 | {MEMSTICK_MATCH_ALL, MEMSTICK_TYPE_PRO, MEMSTICK_CATEGORY_STORAGE_DUO, |
1354 | MEMSTICK_CLASS_DUO}, |
1355 | {} |
1356 | }; |
1357 | |
1358 | |
1359 | static struct memstick_driver mspro_block_driver = { |
1360 | .driver = { |
1361 | .name = DRIVER_NAME, |
1362 | .owner = THIS_MODULE |
1363 | }, |
1364 | .id_table = mspro_block_id_tbl, |
1365 | .probe = mspro_block_probe, |
1366 | .remove = mspro_block_remove, |
1367 | .suspend = mspro_block_suspend, |
1368 | .resume = mspro_block_resume |
1369 | }; |
1370 | |
1371 | static int __init mspro_block_init(void) |
1372 | { |
1373 | int rc = -ENOMEM; |
1374 | |
1375 | rc = register_blkdev(major, DRIVER_NAME); |
1376 | if (rc < 0) { |
1377 | printk(KERN_ERR DRIVER_NAME ": failed to register " |
1378 | "major %d, error %d\n" , major, rc); |
1379 | return rc; |
1380 | } |
1381 | if (!major) |
1382 | major = rc; |
1383 | |
1384 | rc = memstick_register_driver(drv: &mspro_block_driver); |
1385 | if (rc) |
1386 | unregister_blkdev(major, DRIVER_NAME); |
1387 | return rc; |
1388 | } |
1389 | |
1390 | static void __exit mspro_block_exit(void) |
1391 | { |
1392 | memstick_unregister_driver(drv: &mspro_block_driver); |
1393 | unregister_blkdev(major, DRIVER_NAME); |
1394 | idr_destroy(&mspro_block_disk_idr); |
1395 | } |
1396 | |
1397 | module_init(mspro_block_init); |
1398 | module_exit(mspro_block_exit); |
1399 | |
1400 | MODULE_LICENSE("GPL" ); |
1401 | MODULE_AUTHOR("Alex Dubov" ); |
1402 | MODULE_DESCRIPTION("Sony MemoryStickPro block device driver" ); |
1403 | MODULE_DEVICE_TABLE(memstick, mspro_block_id_tbl); |
1404 | |