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 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) 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; 100 writeb_relaxed(*payload, sth->base + reg); 101 break; 102 103 case STP_PACKET_MERR: 104 if (size > 4) 105 size = 4; 106 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 116 size = 0; 117 writeb_relaxed(0, outp); 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; 138 default: 139 return -ENOTSUPP; 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)); 205 if (!base) 206 return -ENOMEM; 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)); 213 if (!channels) 214 return -ENOMEM; 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>"); 268