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