optimised.c 6.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256
  1. #define PY_SSIZE_T_CLEAN
  2. #include "Python.h"
  3. #include <stdio.h>
  4. #include <stdint.h>
  5. #define FLUXOP_INDEX 1
  6. #define FLUXOP_SPACE 2
  7. #define FLUXOP_ASTABLE 3
  8. /* bitarray.append(value) */
  9. static PyObject *append_s;
  10. static int bitarray_append(PyObject *bitarray, PyObject *value)
  11. {
  12. PyObject *res = PyObject_CallMethodObjArgs(
  13. bitarray, append_s, value, NULL);
  14. if (res == NULL)
  15. return 0;
  16. Py_DECREF(res);
  17. return 1;
  18. }
  19. /* Like PyList_Append() but steals a reference to @item. */
  20. static int PyList_Append_SR(PyObject *list, PyObject *item)
  21. {
  22. int rc = PyList_Append(list, item);
  23. Py_DECREF(item);
  24. return rc;
  25. }
  26. static PyObject *
  27. flux_to_bitcells(PyObject *self, PyObject *args)
  28. {
  29. /* Parameters */
  30. PyObject *bit_array, *time_array, *revolutions;
  31. PyObject *index_iter, *flux_iter;
  32. double freq, clock_centre, clock_min, clock_max;
  33. double pll_period_adj, pll_phase_adj;
  34. /* Local variables */
  35. PyObject *item;
  36. double clock, new_ticks, ticks, to_index;
  37. int zeros, nbits;
  38. if (!PyArg_ParseTuple(args, "OOOOOdddddd",
  39. &bit_array, &time_array, &revolutions,
  40. &index_iter, &flux_iter,
  41. &freq, &clock_centre, &clock_min, &clock_max,
  42. &pll_period_adj, &pll_phase_adj))
  43. return NULL;
  44. nbits = 0;
  45. ticks = 0.0;
  46. clock = clock_centre;
  47. /* to_index = next(index_iter) */
  48. item = PyIter_Next(index_iter);
  49. to_index = PyFloat_AsDouble(item);
  50. Py_DECREF(item);
  51. if (PyErr_Occurred())
  52. return NULL;
  53. /* for x in flux_iter: */
  54. assert(PyIter_Check(flux_iter));
  55. while ((item = PyIter_Next(flux_iter)) != NULL) {
  56. double x = PyFloat_AsDouble(item);
  57. Py_DECREF(item);
  58. if (PyErr_Occurred())
  59. return NULL;
  60. /* Gather enough ticks to generate at least one bitcell. */
  61. ticks += x / freq;
  62. if (ticks < clock/2)
  63. continue;
  64. /* Clock out zero or more 0s, followed by a 1. */
  65. for (zeros = 0; ; zeros++) {
  66. /* Check if we cross the index mark. */
  67. to_index -= clock;
  68. if (to_index < 0) {
  69. if (PyList_Append_SR(revolutions, PyLong_FromLong(nbits)) < 0)
  70. return NULL;
  71. nbits = 0;
  72. item = PyIter_Next(index_iter);
  73. to_index += PyFloat_AsDouble(item);
  74. Py_DECREF(item);
  75. if (PyErr_Occurred())
  76. return NULL;
  77. }
  78. nbits += 1;
  79. ticks -= clock;
  80. if (PyList_Append_SR(time_array, PyFloat_FromDouble(clock)) < 0)
  81. return NULL;
  82. if (ticks < clock/2) {
  83. if (!bitarray_append(bit_array, Py_True))
  84. return NULL;
  85. break;
  86. }
  87. if (!bitarray_append(bit_array, Py_False))
  88. return NULL;
  89. }
  90. /* PLL: Adjust clock frequency according to phase mismatch. */
  91. if (zeros <= 3) {
  92. /* In sync: adjust clock by a fraction of the phase mismatch. */
  93. clock += ticks * pll_period_adj;
  94. } else {
  95. /* Out of sync: adjust clock towards centre. */
  96. clock += (clock_centre - clock) * pll_period_adj;
  97. }
  98. /* Clamp the clock's adjustment range. */
  99. if (clock < clock_min)
  100. clock = clock_min;
  101. else if (clock > clock_max)
  102. clock = clock_max;
  103. /* PLL: Adjust clock phase according to mismatch. */
  104. new_ticks = ticks * (1.0 - pll_phase_adj);
  105. if (PyList_SetItem(time_array, PyList_Size(time_array)-1,
  106. PyFloat_FromDouble(ticks - new_ticks)) < 0)
  107. return NULL;
  108. ticks = new_ticks;
  109. }
  110. Py_RETURN_NONE;
  111. }
  112. static int _read_28bit(uint8_t *p)
  113. {
  114. int x;
  115. x = (p[0] ) >> 1;
  116. x |= (p[1] & 0xfe) << 6;
  117. x |= (p[2] & 0xfe) << 13;
  118. x |= (p[3] & 0xfe) << 20;
  119. return x;
  120. }
  121. static PyObject *
  122. decode_flux(PyObject *self, PyObject *args)
  123. {
  124. /* Parameters */
  125. Py_buffer bytearray;
  126. PyObject *res = NULL;
  127. /* bytearray buffer */
  128. uint8_t *p;
  129. Py_ssize_t l;
  130. /* Local variables */
  131. PyObject *flux, *index;
  132. long val, ticks, ticks_since_index;
  133. int i, opcode;
  134. if (!PyArg_ParseTuple(args, "y*", &bytearray))
  135. return NULL;
  136. p = bytearray.buf;
  137. l = bytearray.len;
  138. /* assert dat[-1] == 0 */
  139. if ((l == 0) || (p[l-1] != 0)) {
  140. PyErr_SetString(PyExc_ValueError, "Flux is not NUL-terminated");
  141. PyBuffer_Release(&bytearray);
  142. return NULL;
  143. }
  144. /* len(dat) -= 1 */
  145. l -= 1;
  146. /* flux, index = [], [] */
  147. flux = PyList_New(0);
  148. index = PyList_New(0);
  149. /* ticks, ticks_since_index = 0, 0 */
  150. ticks = 0;
  151. ticks_since_index = 0;
  152. while (l != 0) {
  153. i = *p++;
  154. if (i == 255) {
  155. if ((l -= 2) < 0)
  156. goto oos;
  157. opcode = *p++;
  158. switch (opcode) {
  159. case FLUXOP_INDEX:
  160. if ((l -= 4) < 0)
  161. goto oos;
  162. val = _read_28bit(p);
  163. p += 4;
  164. if (PyList_Append_SR(
  165. index, PyLong_FromLong(
  166. ticks_since_index + ticks + val)) < 0)
  167. goto out;
  168. ticks_since_index = -(ticks + val);
  169. break;
  170. case FLUXOP_SPACE:
  171. if ((l -= 4) < 0)
  172. goto oos;
  173. ticks += _read_28bit(p);
  174. p += 4;
  175. break;
  176. default:
  177. PyErr_Format(PyExc_ValueError,
  178. "Bad opcode in flux stream (%d)", opcode);
  179. goto out;
  180. }
  181. } else {
  182. if (i < 250) {
  183. l -= 1;
  184. val = i;
  185. } else {
  186. if ((l -= 2) < 0)
  187. goto oos;
  188. val = 250 + (i - 250) * 255;
  189. val += *p++ - 1;
  190. }
  191. ticks += val;
  192. if (PyList_Append_SR(flux, PyLong_FromLong(ticks)) < 0)
  193. goto out;
  194. ticks_since_index += ticks;
  195. ticks = 0;
  196. }
  197. }
  198. res = Py_BuildValue("OO", flux, index);
  199. out:
  200. PyBuffer_Release(&bytearray);
  201. Py_DECREF(flux);
  202. Py_DECREF(index);
  203. return res;
  204. oos:
  205. PyErr_SetString(PyExc_ValueError, "Unexpected end of flux");
  206. goto out;
  207. }
  208. static PyMethodDef modulefuncs[] = {
  209. { "flux_to_bitcells", flux_to_bitcells, METH_VARARGS, NULL },
  210. { "decode_flux", decode_flux, METH_VARARGS, NULL },
  211. { NULL }
  212. };
  213. static PyModuleDef moduledef = {
  214. PyModuleDef_HEAD_INIT, "optimised", 0, -1, modulefuncs,
  215. };
  216. PyMODINIT_FUNC PyInit_optimised(void)
  217. {
  218. append_s = Py_BuildValue("s", "append");
  219. return PyModule_Create(&moduledef);
  220. }