1
2
3
4
5
6
7
8
9#include <linux/mtd/mtd.h>
10#include <linux/mtd/map.h>
11#include <linux/mtd/cfi.h>
12#include <linux/time.h>
13
14#include "cobalt-flash.h"
15
16#define ADRS(offset) (COBALT_BUS_FLASH_BASE + offset)
17
18static struct map_info cobalt_flash_map = {
19 .name = "cobalt-flash",
20 .bankwidth = 2,
21 .size = 0x4000000,
22 .phys = 0,
23};
24
25static map_word flash_read16(struct map_info *map, unsigned long offset)
26{
27 map_word r;
28
29 r.x[0] = cobalt_bus_read32(map->virt, ADRS(offset));
30 if (offset & 0x2)
31 r.x[0] >>= 16;
32 else
33 r.x[0] &= 0x0000ffff;
34
35 return r;
36}
37
38static void flash_write16(struct map_info *map, const map_word datum,
39 unsigned long offset)
40{
41 u16 data = (u16)datum.x[0];
42
43 cobalt_bus_write16(map->virt, ADRS(offset), data);
44}
45
46static void flash_copy_from(struct map_info *map, void *to,
47 unsigned long from, ssize_t len)
48{
49 u32 src = from;
50 u8 *dest = to;
51 u32 data;
52
53 while (len) {
54 data = cobalt_bus_read32(map->virt, ADRS(src));
55 do {
56 *dest = data >> (8 * (src & 3));
57 src++;
58 dest++;
59 len--;
60 } while (len && (src % 4));
61 }
62}
63
64static void flash_copy_to(struct map_info *map, unsigned long to,
65 const void *from, ssize_t len)
66{
67 const u8 *src = from;
68 u32 dest = to;
69
70 pr_info("%s: offset 0x%x: length %zu\n", __func__, dest, len);
71 while (len) {
72 u16 data;
73
74 do {
75 data = *src << (8 * (dest & 1));
76 src++;
77 dest++;
78 len--;
79 } while (len && (dest % 2));
80
81 cobalt_bus_write16(map->virt, ADRS(dest - 2), data);
82 }
83}
84
85int cobalt_flash_probe(struct cobalt *cobalt)
86{
87 struct map_info *map = &cobalt_flash_map;
88 struct mtd_info *mtd;
89
90 BUG_ON(!map_bankwidth_supported(map->bankwidth));
91 map->virt = cobalt->bar1;
92 map->read = flash_read16;
93 map->write = flash_write16;
94 map->copy_from = flash_copy_from;
95 map->copy_to = flash_copy_to;
96
97 mtd = do_map_probe("cfi_probe", map);
98 cobalt->mtd = mtd;
99 if (!mtd) {
100 cobalt_err("Probe CFI flash failed!\n");
101 return -1;
102 }
103
104 mtd->owner = THIS_MODULE;
105 mtd->dev.parent = &cobalt->pci_dev->dev;
106 mtd_device_register(mtd, NULL, 0);
107 return 0;
108}
109
110void cobalt_flash_remove(struct cobalt *cobalt)
111{
112 if (cobalt->mtd) {
113 mtd_device_unregister(cobalt->mtd);
114 map_destroy(cobalt->mtd);
115 }
116}
117