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
 29struct 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
 38static struct intel_th_channel __iomem *
 39sth_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
 47static 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
 70static ssize_t sth_stm_packet(struct stm_data *stm_data, unsigned int master,
 71			      unsigned int channel, unsigned int packet,
 72			      unsigned int flags, unsigned int size,
 73			      const unsigned char *payload)
 74{
 75	struct sth_device *sth = container_of(stm_data, struct sth_device, stm);
 76	struct intel_th_channel __iomem *out =
 77		sth_channel(sth, master, channel);
 78	u64 __iomem *outp = &out->Dn;
 79	unsigned long reg = REG_STH_TRIG;
 80
 81#ifndef CONFIG_64BIT
 82	if (size > 4)
 83		size = 4;
 84#endif
 85
 86	size = rounddown_pow_of_two(size);
 87
 88	switch (packet) {
 89	/* Global packets (GERR, XSYNC, TRIG) are sent with register writes */
 90	case STP_PACKET_GERR:
 91		reg += 4;
 92	case STP_PACKET_XSYNC:
 93		reg += 8;
 94	case STP_PACKET_TRIG:
 95		if (flags & STP_PACKET_TIMESTAMPED)
 96			reg += 4;
 97		writeb_relaxed(*payload, sth->base + reg);
 98		break;
 99
100	case STP_PACKET_MERR:
101		if (size > 4)
102			size = 4;
103
104		sth_iowrite(&out->MERR, payload, size);
105		break;
106
107	case STP_PACKET_FLAG:
108		if (flags & STP_PACKET_TIMESTAMPED)
109			outp = (u64 __iomem *)&out->FLAG_TS;
110		else
111			outp = (u64 __iomem *)&out->FLAG;
112
113		size = 0;
114		writeb_relaxed(0, outp);
115		break;
116
117	case STP_PACKET_USER:
118		if (flags & STP_PACKET_TIMESTAMPED)
119			outp = &out->USER_TS;
120		else
121			outp = &out->USER;
122		sth_iowrite(outp, payload, size);
123		break;
124
125	case STP_PACKET_DATA:
126		outp = &out->Dn;
127
128		if (flags & STP_PACKET_TIMESTAMPED)
129			outp += 2;
130		if (flags & STP_PACKET_MARKED)
131			outp++;
132
133		sth_iowrite(outp, payload, size);
134		break;
135	default:
136		return -ENOTSUPP;
137	}
138
139	return size;
140}
141
142static phys_addr_t
143sth_stm_mmio_addr(struct stm_data *stm_data, unsigned int master,
144		  unsigned int channel, unsigned int nr_chans)
145{
146	struct sth_device *sth = container_of(stm_data, struct sth_device, stm);
147	phys_addr_t addr;
148
149	master -= sth->stm.sw_start;
150	addr = sth->channels_phys + (master * sth->stm.sw_nchannels + channel) *
151		sizeof(struct intel_th_channel);
152
153	if (offset_in_page(addr) ||
154	    offset_in_page(nr_chans * sizeof(struct intel_th_channel)))
155		return 0;
156
157	return addr;
158}
159
160static int sth_stm_link(struct stm_data *stm_data, unsigned int master,
161			 unsigned int channel)
162{
163	struct sth_device *sth = container_of(stm_data, struct sth_device, stm);
164
165	intel_th_set_output(to_intel_th_device(sth->dev), master);
166
167	return 0;
168}
169
170static int intel_th_sw_init(struct sth_device *sth)
171{
172	u32 reg;
173
174	reg = ioread32(sth->base + REG_STH_STHCAP1);
175	sth->stm.sw_nchannels = reg & 0xff;
176
177	reg = ioread32(sth->base + REG_STH_STHCAP0);
178	sth->stm.sw_start = reg & 0xffff;
179	sth->stm.sw_end = reg >> 16;
180
181	sth->sw_nmasters = sth->stm.sw_end - sth->stm.sw_start;
182	dev_dbg(sth->dev, "sw_start: %x sw_end: %x masters: %x nchannels: %x\n",
183		sth->stm.sw_start, sth->stm.sw_end, sth->sw_nmasters,
184		sth->stm.sw_nchannels);
185
186	return 0;
187}
188
189static int intel_th_sth_probe(struct intel_th_device *thdev)
190{
191	struct device *dev = &thdev->dev;
192	struct sth_device *sth;
193	struct resource *res;
194	void __iomem *base, *channels;
195	int err;
196
197	res = intel_th_device_get_resource(thdev, IORESOURCE_MEM, 0);
198	if (!res)
199		return -ENODEV;
200
201	base = devm_ioremap(dev, res->start, resource_size(res));
202	if (!base)
203		return -ENOMEM;
204
205	res = intel_th_device_get_resource(thdev, IORESOURCE_MEM, 1);
206	if (!res)
207		return -ENODEV;
208
209	channels = devm_ioremap(dev, res->start, resource_size(res));
210	if (!channels)
211		return -ENOMEM;
212
213	sth = devm_kzalloc(dev, sizeof(*sth), GFP_KERNEL);
214	if (!sth)
215		return -ENOMEM;
216
217	sth->dev = dev;
218	sth->base = base;
219	sth->channels = channels;
220	sth->channels_phys = res->start;
221	sth->stm.name = dev_name(dev);
222	sth->stm.packet = sth_stm_packet;
223	sth->stm.mmio_addr = sth_stm_mmio_addr;
224	sth->stm.sw_mmiosz = sizeof(struct intel_th_channel);
225	sth->stm.link = sth_stm_link;
226
227	err = intel_th_sw_init(sth);
228	if (err)
229		return err;
230
231	err = stm_register_device(dev, &sth->stm, THIS_MODULE);
232	if (err) {
233		dev_err(dev, "stm_register_device failed\n");
234		return err;
235	}
236
237	dev_set_drvdata(dev, sth);
238
239	return 0;
240}
241
242static void intel_th_sth_remove(struct intel_th_device *thdev)
243{
244	struct sth_device *sth = dev_get_drvdata(&thdev->dev);
245
246	stm_unregister_device(&sth->stm);
247}
248
249static struct intel_th_driver intel_th_sth_driver = {
250	.probe	= intel_th_sth_probe,
251	.remove	= intel_th_sth_remove,
252	.driver	= {
253		.name	= "sth",
254		.owner	= THIS_MODULE,
255	},
256};
257
258module_driver(intel_th_sth_driver,
259	      intel_th_driver_register,
260	      intel_th_driver_unregister);
261
262MODULE_LICENSE("GPL v2");
263MODULE_DESCRIPTION("Intel(R) Trace Hub Software Trace Hub driver");
264MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@intel.com>");