drivers/hwtracing/intel_th/sth.c

   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 <[email protected]>");