diff --git a/drivers/usb/gadget/function/u_audio.c b/drivers/usb/gadget/function/u_audio.c index a604ca242514..abd0089e8a11 100644 --- a/drivers/usb/gadget/function/u_audio.c +++ b/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); diff --git a/drivers/usb/gadget/function/u_audio.h b/drivers/usb/gadget/function/u_audio.h index 369261e8bdd8..49764d1c09ee 100644 --- a/drivers/usb/gadget/function/u_audio.h +++ b/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;