usb: gadget: u_audio: add uevent for ppm compensation

This patch add uevent to notify the application layer how much ppm
is different between USB clk and AUDIO clk.

The event include two parts USB_STATE and PPM. For example:

  g_audio_work: sent uac uevent USB_STATE=SET_AUDIO_CLK PPM=12
  g_audio_work: sent uac uevent USB_STATE=SET_AUDIO_CLK PPM=-1

Note: The ppm compensation depends on the method implement of
clk drift and compensation in the rockchip_pdm.c driver. So if
you want the ppm compensation to take effect, please make sure
the commit "ASoC: rockchip: pdm: Add support for clk compensation"
is merged.

Change-Id: Id25411397fe376342c773c11f1989ed5854f8ad9
Signed-off-by: Ren Jianing <jianing.ren@rock-chips.com>
Signed-off-by: Frank Wang <frank.wang@rock-chips.com>

drivers/usb/gadget/function/u_audio.c

@@ -25,6 +25,8 @@
 #define PRD_SIZE_MAX	PAGE_SIZE
 #define MIN_PERIODS	4
 
+#define CLK_PPM_GROUP_SIZE	20
+
 static struct class *audio_class;
 
 /* Runtime data params for one stream */
@@ -544,7 +546,7 @@ int u_audio_start_playback(struct g_audio *audio_dev)
 {
 	struct snd_uac_chip *uac = audio_dev->uac;
 	struct usb_gadget *gadget = audio_dev->gadget;
-	struct device *dev = &gadget->dev;
+	struct device *dev = audio_dev->device;
 	struct usb_request *req;
 	struct usb_ep *ep;
 	struct uac_rtd_params *prm;
@@ -638,6 +640,10 @@ EXPORT_SYMBOL_GPL(u_audio_fu_set_cmd);
 
 int u_audio_fu_get_cmd(struct usb_audio_control *con, u8 cmd)
 {
+	struct g_audio *audio_dev = (struct g_audio *)con->context;
+
+	dev_dbg(audio_dev->device, "GET_CMD con %s cmd %d data %d\n",
+		con->name, cmd, (int16_t)con->data[cmd]);
 	return con->data[cmd];
 }
 EXPORT_SYMBOL_GPL(u_audio_fu_get_cmd);
@@ -646,8 +652,6 @@ static void g_audio_work(struct work_struct *data)
 {
 	struct g_audio *audio = container_of(data, struct g_audio, work);
 	struct uac_params *params = &audio->params;
-	struct usb_gadget *gadget = audio->gadget;
-	struct device *dev = &gadget->dev;
 	char *uac_event[4]  = { NULL, NULL, NULL, NULL };
 	char str[19];
 	signed short volume;
@@ -712,6 +716,10 @@ static void g_audio_work(struct work_struct *data)
 			snprintf(str, sizeof(str), "VOLUME=%d%%", volume + 50);
 			uac_event[2] = str;
 			break;
+		case SET_AUDIO_CLK:
+			uac_event[0] = "USB_STATE=SET_AUDIO_CLK";
+			snprintf(str, sizeof(str), "PPM=%d", params->ppm);
+			uac_event[1] = str;
 		default:
 			break;
 		}
@@ -719,11 +727,112 @@ static void g_audio_work(struct work_struct *data)
 		audio->usb_state[i] = false;
 		kobject_uevent_env(&audio->device->kobj, KOBJ_CHANGE,
 				   uac_event);
-		dev_dbg(dev, "%s: sent uac uevent %s %s %s\n", __func__,
-			uac_event[0], uac_event[1], uac_event[2]);
+		dev_dbg(audio->device, "%s: sent uac uevent %s %s %s\n",
+			__func__, uac_event[0], uac_event[1], uac_event[2]);
 	}
 }
 
+static void ppm_calculate_work(struct work_struct *data)
+{
+	struct g_audio *g_audio = container_of(data, struct g_audio,
+					       ppm_work.work);
+	struct usb_gadget *gadget = g_audio->gadget;
+	uint32_t frame_number, fn_msec, clk_msec;
+	struct frame_number_data *fn = g_audio->fn;
+	uint64_t time_now, time_msec_tmp;
+	int32_t ppm;
+	static int32_t ppms[CLK_PPM_GROUP_SIZE];
+	static int32_t ppm_sum;
+	int32_t cnt = fn->second % CLK_PPM_GROUP_SIZE;
+
+	time_now = ktime_get_raw();
+	frame_number = gadget->ops->get_frame(gadget);
+
+	if (g_audio->fn->time_last &&
+	    time_now - g_audio->fn->time_last > 1500000000ULL)
+		dev_warn(g_audio->device, "PPM work scheduled too slow!\n");
+
+	g_audio->fn->time_last = time_now;
+
+	/*
+	 * If usb is disconnected, the controller will not receive the
+	 * SoF signal and frame number will be invalid. Because we can't
+	 * get accurate time of disconnect and whether the gadget will be
+	 * plugged into the same host next time or not. We must clear all
+	 * statistics.
+	 */
+	if (gadget->state != USB_STATE_CONFIGURED) {
+		memset(g_audio->fn, 0, sizeof(*g_audio->fn));
+		dev_dbg(g_audio->device, "Disconnect. frame number is cleared\n");
+		goto out;
+	}
+
+	/* Fist statistic to record begin frame number and system time */
+	if (!g_audio->fn->second++) {
+		g_audio->fn->time_begin = g_audio->fn->time_last;
+		g_audio->fn->fn_begin = frame_number;
+		g_audio->fn->fn_last = frame_number;
+		goto out;
+	}
+
+	/*
+	 * For DWC3 Controller, only 13 bits is used to store frame(micro)
+	 * number. In other words, the frame number will overflow at most
+	 * 2.047 seconds. We add another registor fn_overflow the record
+	 * total frame number.
+	 */
+	if (frame_number <= g_audio->fn->fn_last)
+		g_audio->fn->fn_overflow++;
+	g_audio->fn->fn_last = frame_number;
+
+	if (!g_audio->fn->fn_overflow)
+		goto out;
+
+	/* The lower 3 bits represent micro number frame, we don't need it */
+	fn_msec = (((fn->fn_overflow - 1) << 14) +
+		   (BIT(14) + fn->fn_last - fn->fn_begin) + BIT(2)) >> 3;
+	time_msec_tmp = fn->time_last - fn->time_begin + 500000ULL;
+	do_div(time_msec_tmp, 1000000U);
+	clk_msec = (uint32_t)time_msec_tmp;
+
+	/*
+	 * According to the definition of ppm:
+	 *   host_clk = (1 + ppm / 1000000) * gadget_clk
+	 * we can get:
+	 *   ppm = (host_clk - gadget_clk) * 1000000 / gadget_clk
+	 */
+	ppm = (fn_msec > clk_msec) ?
+	      (fn_msec - clk_msec) * 1000000L / clk_msec :
+	      -((clk_msec - fn_msec) * 1000000L / clk_msec);
+
+	ppm_sum = ppm_sum - ppms[cnt] + ppm;
+	ppms[cnt] = ppm;
+
+	dev_dbg(g_audio->device,
+		"frame %u msec %u ppm_calc %d ppm_avage(%d) %d\n",
+		fn_msec, clk_msec, ppm, CLK_PPM_GROUP_SIZE,
+		ppm_sum / CLK_PPM_GROUP_SIZE);
+
+	/*
+	 * We calculate the average of ppm over a period of time. If the
+	 * latest frame number is too far from the average, no event will
+	 * be sent.
+	 */
+	if (abs(ppm_sum / CLK_PPM_GROUP_SIZE - ppm) < 3) {
+		ppm = ppm_sum > 0 ?
+		      (ppm_sum + CLK_PPM_GROUP_SIZE / 2) / CLK_PPM_GROUP_SIZE :
+		      (ppm_sum - CLK_PPM_GROUP_SIZE / 2) / CLK_PPM_GROUP_SIZE;
+		if (ppm != g_audio->params.ppm) {
+			g_audio->params.ppm = ppm;
+			g_audio->usb_state[SET_AUDIO_CLK] = true;
+			schedule_work(&g_audio->work);
+		}
+	}
+
+out:
+	schedule_delayed_work(&g_audio->ppm_work, 1 * HZ);
+}
+
 int g_audio_setup(struct g_audio *g_audio, const char *pcm_name,
 					const char *card_name)
 {
@@ -748,6 +857,12 @@ int g_audio_setup(struct g_audio *g_audio, const char *pcm_name,
 	p_chmask = params->p_chmask;
 	c_chmask = params->c_chmask;
 
+	g_audio->fn = kzalloc(sizeof(*g_audio->fn), GFP_KERNEL);
+	if (!g_audio->fn) {
+		err = -ENOMEM;
+		goto fail;
+	}
+
 	if (c_chmask) {
 		struct uac_rtd_params *prm = &uac->c_prm;
 
@@ -845,6 +960,8 @@ int g_audio_setup(struct g_audio *g_audio, const char *pcm_name,
 	}
 
 	INIT_WORK(&g_audio->work, g_audio_work);
+	INIT_DELAYED_WORK(&g_audio->ppm_work, ppm_calculate_work);
+	ppm_calculate_work(&g_audio->ppm_work.work);
 
 	if (!err)
 		return 0;
@@ -857,6 +974,7 @@ fail:
 	kfree(uac->p_prm.rbuf);
 	kfree(uac->c_prm.rbuf);
 	kfree(uac);
+	kfree(g_audio->fn);
 
 	return err;
 }
@@ -871,6 +989,7 @@ void g_audio_cleanup(struct g_audio *g_audio)
 		return;
 
 	cancel_work_sync(&g_audio->work);
+	cancel_delayed_work_sync(&g_audio->ppm_work);
 	device_destroy(g_audio->device->class, g_audio->device->devt);
 	g_audio->device = NULL;
 
@@ -879,11 +998,15 @@ void g_audio_cleanup(struct g_audio *g_audio)
 	if (card)
 		snd_card_free(card);
 
+	free_ep(&uac->c_prm, g_audio->out_ep);
+	free_ep(&uac->p_prm, g_audio->in_ep);
+
 	kfree(uac->p_prm.reqs);
 	kfree(uac->c_prm.reqs);
 	kfree(uac->p_prm.rbuf);
 	kfree(uac->c_prm.rbuf);
 	kfree(uac);
+	kfree(g_audio->fn);
 }
 EXPORT_SYMBOL_GPL(g_audio_cleanup);
 

drivers/usb/gadget/function/u_audio.h

@@ -34,6 +34,8 @@ struct uac_params {
 	int c_srate_active;		/* selected rate in Hz */
 	int c_ssize;	/* sample size */
 
+	int ppm;	/* difference between audio clk and usb clk */
+
 	int req_number; /* number of preallocated requests */
 };
 
@@ -46,6 +48,7 @@ enum usb_state_index {
 	SET_VOLUME_IN,
 	SET_MUTE_OUT,
 	SET_MUTE_IN,
+	SET_AUDIO_CLK,
 	SET_USB_STATE_MAX,
 };
 
@@ -54,12 +57,24 @@ enum stream_state_index {
 	STATE_IN,
 };
 
+struct frame_number_data {
+	uint32_t fn_begin;	/* frame number when starting statistics */
+	uint32_t fn_last;	/* frame number in the latest statistics */
+	uint32_t fn_overflow;	/* the time of frame number overflow */
+	uint32_t second;	/* total seconds counted */
+	ktime_t time_begin;	/* system time when starting statistics */
+	ktime_t time_last;	/* system time in the latest statistics */
+};
+
 struct g_audio {
 	struct device *device;
 	bool usb_state[SET_USB_STATE_MAX];
 	bool stream_state[2];
 	struct work_struct work;
 
+	struct frame_number_data *fn;
+	struct delayed_work ppm_work;
+
 	struct usb_function func;
 	struct usb_gadget *gadget;