Alexander Shishkin | f04e449 | 2015-09-22 15:47:17 +0300 | [diff] [blame] | 1 | /* |
| 2 | * Intel(R) Trace Hub Software Trace Hub support |
| 3 | * |
| 4 | * Copyright (C) 2014-2015 Intel Corporation. |
| 5 | * |
| 6 | * This program is free software; you can redistribute it and/or modify it |
| 7 | * under the terms and conditions of the GNU General Public License, |
| 8 | * version 2, as published by the Free Software Foundation. |
| 9 | * |
| 10 | * This program is distributed in the hope it will be useful, but WITHOUT |
| 11 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or |
| 12 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for |
| 13 | * more details. |
| 14 | */ |
| 15 | |
| 16 | #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt |
| 17 | |
| 18 | #include <linux/types.h> |
| 19 | #include <linux/module.h> |
| 20 | #include <linux/device.h> |
| 21 | #include <linux/io.h> |
| 22 | #include <linux/mm.h> |
| 23 | #include <linux/slab.h> |
| 24 | #include <linux/stm.h> |
| 25 | |
| 26 | #include "intel_th.h" |
| 27 | #include "sth.h" |
| 28 | |
| 29 | struct sth_device { |
| 30 | void __iomem *base; |
| 31 | void __iomem *channels; |
| 32 | phys_addr_t channels_phys; |
| 33 | struct device *dev; |
| 34 | struct stm_data stm; |
| 35 | unsigned int sw_nmasters; |
| 36 | }; |
| 37 | |
| 38 | static struct intel_th_channel __iomem * |
| 39 | sth_channel(struct sth_device *sth, unsigned int master, unsigned int channel) |
| 40 | { |
| 41 | struct intel_th_channel __iomem *sw_map = sth->channels; |
| 42 | |
| 43 | return &sw_map[(master - sth->stm.sw_start) * sth->stm.sw_nchannels + |
| 44 | channel]; |
| 45 | } |
| 46 | |
| 47 | static void sth_iowrite(void __iomem *dest, const unsigned char *payload, |
| 48 | unsigned int size) |
| 49 | { |
| 50 | switch (size) { |
| 51 | #ifdef CONFIG_64BIT |
| 52 | case 8: |
| 53 | writeq_relaxed(*(u64 *)payload, dest); |
| 54 | break; |
| 55 | #endif |
| 56 | case 4: |
| 57 | writel_relaxed(*(u32 *)payload, dest); |
| 58 | break; |
| 59 | case 2: |
| 60 | writew_relaxed(*(u16 *)payload, dest); |
| 61 | break; |
| 62 | case 1: |
| 63 | writeb_relaxed(*(u8 *)payload, dest); |
| 64 | break; |
| 65 | default: |
| 66 | break; |
| 67 | } |
| 68 | } |
| 69 | |
Chunyan Zhang | 22975be | 2016-11-21 15:57:21 +0800 | [diff] [blame] | 70 | static ssize_t notrace sth_stm_packet(struct stm_data *stm_data, |
| 71 | unsigned int master, |
| 72 | unsigned int channel, |
| 73 | unsigned int packet, |
| 74 | unsigned int flags, |
| 75 | unsigned int size, |
| 76 | const unsigned char *payload) |
Alexander Shishkin | f04e449 | 2015-09-22 15:47:17 +0300 | [diff] [blame] | 77 | { |
| 78 | struct sth_device *sth = container_of(stm_data, struct sth_device, stm); |
| 79 | struct intel_th_channel __iomem *out = |
| 80 | sth_channel(sth, master, channel); |
| 81 | u64 __iomem *outp = &out->Dn; |
| 82 | unsigned long reg = REG_STH_TRIG; |
| 83 | |
| 84 | #ifndef CONFIG_64BIT |
| 85 | if (size > 4) |
| 86 | size = 4; |
| 87 | #endif |
| 88 | |
| 89 | size = rounddown_pow_of_two(size); |
| 90 | |
| 91 | switch (packet) { |
| 92 | /* Global packets (GERR, XSYNC, TRIG) are sent with register writes */ |
| 93 | case STP_PACKET_GERR: |
| 94 | reg += 4; |
| 95 | case STP_PACKET_XSYNC: |
| 96 | reg += 8; |
| 97 | case STP_PACKET_TRIG: |
| 98 | if (flags & STP_PACKET_TIMESTAMPED) |
| 99 | reg += 4; |
Alexander Shishkin | 9700750 | 2016-02-15 19:11:58 +0200 | [diff] [blame] | 100 | writeb_relaxed(*payload, sth->base + reg); |
Alexander Shishkin | f04e449 | 2015-09-22 15:47:17 +0300 | [diff] [blame] | 101 | break; |
| 102 | |
| 103 | case STP_PACKET_MERR: |
Alexander Shishkin | 9700750 | 2016-02-15 19:11:58 +0200 | [diff] [blame] | 104 | if (size > 4) |
| 105 | size = 4; |
| 106 | |
Alexander Shishkin | f04e449 | 2015-09-22 15:47:17 +0300 | [diff] [blame] | 107 | sth_iowrite(&out->MERR, payload, size); |
| 108 | break; |
| 109 | |
| 110 | case STP_PACKET_FLAG: |
| 111 | if (flags & STP_PACKET_TIMESTAMPED) |
| 112 | outp = (u64 __iomem *)&out->FLAG_TS; |
| 113 | else |
| 114 | outp = (u64 __iomem *)&out->FLAG; |
| 115 | |
Alexander Shishkin | 9700750 | 2016-02-15 19:11:58 +0200 | [diff] [blame] | 116 | size = 0; |
| 117 | writeb_relaxed(0, outp); |
Alexander Shishkin | f04e449 | 2015-09-22 15:47:17 +0300 | [diff] [blame] | 118 | break; |
| 119 | |
| 120 | case STP_PACKET_USER: |
| 121 | if (flags & STP_PACKET_TIMESTAMPED) |
| 122 | outp = &out->USER_TS; |
| 123 | else |
| 124 | outp = &out->USER; |
| 125 | sth_iowrite(outp, payload, size); |
| 126 | break; |
| 127 | |
| 128 | case STP_PACKET_DATA: |
| 129 | outp = &out->Dn; |
| 130 | |
| 131 | if (flags & STP_PACKET_TIMESTAMPED) |
| 132 | outp += 2; |
| 133 | if (flags & STP_PACKET_MARKED) |
| 134 | outp++; |
| 135 | |
| 136 | sth_iowrite(outp, payload, size); |
| 137 | break; |
Alexander Shishkin | 9700750 | 2016-02-15 19:11:58 +0200 | [diff] [blame] | 138 | default: |
| 139 | return -ENOTSUPP; |
Alexander Shishkin | f04e449 | 2015-09-22 15:47:17 +0300 | [diff] [blame] | 140 | } |
| 141 | |
| 142 | return size; |
| 143 | } |
| 144 | |
| 145 | static phys_addr_t |
| 146 | sth_stm_mmio_addr(struct stm_data *stm_data, unsigned int master, |
| 147 | unsigned int channel, unsigned int nr_chans) |
| 148 | { |
| 149 | struct sth_device *sth = container_of(stm_data, struct sth_device, stm); |
| 150 | phys_addr_t addr; |
| 151 | |
| 152 | master -= sth->stm.sw_start; |
| 153 | addr = sth->channels_phys + (master * sth->stm.sw_nchannels + channel) * |
| 154 | sizeof(struct intel_th_channel); |
| 155 | |
| 156 | if (offset_in_page(addr) || |
| 157 | offset_in_page(nr_chans * sizeof(struct intel_th_channel))) |
| 158 | return 0; |
| 159 | |
| 160 | return addr; |
| 161 | } |
| 162 | |
| 163 | static int sth_stm_link(struct stm_data *stm_data, unsigned int master, |
| 164 | unsigned int channel) |
| 165 | { |
| 166 | struct sth_device *sth = container_of(stm_data, struct sth_device, stm); |
| 167 | |
| 168 | intel_th_set_output(to_intel_th_device(sth->dev), master); |
| 169 | |
| 170 | return 0; |
| 171 | } |
| 172 | |
| 173 | static int intel_th_sw_init(struct sth_device *sth) |
| 174 | { |
| 175 | u32 reg; |
| 176 | |
| 177 | reg = ioread32(sth->base + REG_STH_STHCAP1); |
| 178 | sth->stm.sw_nchannels = reg & 0xff; |
| 179 | |
| 180 | reg = ioread32(sth->base + REG_STH_STHCAP0); |
| 181 | sth->stm.sw_start = reg & 0xffff; |
| 182 | sth->stm.sw_end = reg >> 16; |
| 183 | |
| 184 | sth->sw_nmasters = sth->stm.sw_end - sth->stm.sw_start; |
| 185 | dev_dbg(sth->dev, "sw_start: %x sw_end: %x masters: %x nchannels: %x\n", |
| 186 | sth->stm.sw_start, sth->stm.sw_end, sth->sw_nmasters, |
| 187 | sth->stm.sw_nchannels); |
| 188 | |
| 189 | return 0; |
| 190 | } |
| 191 | |
| 192 | static int intel_th_sth_probe(struct intel_th_device *thdev) |
| 193 | { |
| 194 | struct device *dev = &thdev->dev; |
| 195 | struct sth_device *sth; |
| 196 | struct resource *res; |
| 197 | void __iomem *base, *channels; |
| 198 | int err; |
| 199 | |
| 200 | res = intel_th_device_get_resource(thdev, IORESOURCE_MEM, 0); |
| 201 | if (!res) |
| 202 | return -ENODEV; |
| 203 | |
| 204 | base = devm_ioremap(dev, res->start, resource_size(res)); |
Dan Carpenter | 73061da | 2015-10-16 17:09:13 +0300 | [diff] [blame] | 205 | if (!base) |
| 206 | return -ENOMEM; |
Alexander Shishkin | f04e449 | 2015-09-22 15:47:17 +0300 | [diff] [blame] | 207 | |
| 208 | res = intel_th_device_get_resource(thdev, IORESOURCE_MEM, 1); |
| 209 | if (!res) |
| 210 | return -ENODEV; |
| 211 | |
| 212 | channels = devm_ioremap(dev, res->start, resource_size(res)); |
Dan Carpenter | 73061da | 2015-10-16 17:09:13 +0300 | [diff] [blame] | 213 | if (!channels) |
| 214 | return -ENOMEM; |
Alexander Shishkin | f04e449 | 2015-09-22 15:47:17 +0300 | [diff] [blame] | 215 | |
| 216 | sth = devm_kzalloc(dev, sizeof(*sth), GFP_KERNEL); |
| 217 | if (!sth) |
| 218 | return -ENOMEM; |
| 219 | |
| 220 | sth->dev = dev; |
| 221 | sth->base = base; |
| 222 | sth->channels = channels; |
| 223 | sth->channels_phys = res->start; |
| 224 | sth->stm.name = dev_name(dev); |
| 225 | sth->stm.packet = sth_stm_packet; |
| 226 | sth->stm.mmio_addr = sth_stm_mmio_addr; |
| 227 | sth->stm.sw_mmiosz = sizeof(struct intel_th_channel); |
| 228 | sth->stm.link = sth_stm_link; |
| 229 | |
| 230 | err = intel_th_sw_init(sth); |
| 231 | if (err) |
| 232 | return err; |
| 233 | |
| 234 | err = stm_register_device(dev, &sth->stm, THIS_MODULE); |
| 235 | if (err) { |
| 236 | dev_err(dev, "stm_register_device failed\n"); |
| 237 | return err; |
| 238 | } |
| 239 | |
| 240 | dev_set_drvdata(dev, sth); |
| 241 | |
| 242 | return 0; |
| 243 | } |
| 244 | |
| 245 | static void intel_th_sth_remove(struct intel_th_device *thdev) |
| 246 | { |
| 247 | struct sth_device *sth = dev_get_drvdata(&thdev->dev); |
| 248 | |
| 249 | stm_unregister_device(&sth->stm); |
| 250 | } |
| 251 | |
| 252 | static struct intel_th_driver intel_th_sth_driver = { |
| 253 | .probe = intel_th_sth_probe, |
| 254 | .remove = intel_th_sth_remove, |
| 255 | .driver = { |
| 256 | .name = "sth", |
| 257 | .owner = THIS_MODULE, |
| 258 | }, |
| 259 | }; |
| 260 | |
| 261 | module_driver(intel_th_sth_driver, |
| 262 | intel_th_driver_register, |
| 263 | intel_th_driver_unregister); |
| 264 | |
| 265 | MODULE_LICENSE("GPL v2"); |
| 266 | MODULE_DESCRIPTION("Intel(R) Trace Hub Software Trace Hub driver"); |
| 267 | MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@intel.com>"); |